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ABSTRACT 


Vibration  isolation  on  spacecraft  is  needed  for  imaging  sensors,  microgravity 
experiments,  and  other  sensitive  payloads.  The  preferred  method  thus  far  has  been 
passive  isolation  because  of  its  simplicity  and  low  cost.  Active  vibration  isolation  and 
disturbance  rejection  will  soon  be  more  common  as  space  qualified  sensors,  actuators  and 
processors  become  more  capable  and  affordable,  and  performance  requirements  increase. 
Spacecraft  disturbances  are  typically  periodic  vibrations  which  are  most  effectively 
controlled  through  feedforward  techniques.  A  popular  choice  of  feedforward  control 
methods  for  disturbance  rejection  is  the  Multiple  Error  Least  Mean  Squares  (LMS) 
algorithm  which  requires  a  separately  measured  disturbance-correlated  signal  in  its 
implementation.  A  new  technique  called  “Clear  Box”  makes  extensive  use  of 
identification  to  bring  out  information  that  is  normally  hidden  or  not  used  by  traditional 
control  methods.  It  allows  operation  in  an  information-rich  environment  with  built-in 
fault  tolerance,  the  ability  to  control  unanticipated  disturbances,  and  the  ability  to  select 
which  modes  to  control  (if  saturation  of  the  actuators  is  a  possibility  or  concern),  all 
without  the  need  for  a  separately  measured  disturbance-correlated  signal.  Experiments 
using  both  Multiple  Error  LMS  and  Clear  Box  on  an  Ultra  Quiet  Platform  provide  an 
effective  demonstration  of  the  advantages  of  the  Clear  Box  Algorithm,  including  a  new 
Adaptive  Basis  Method  which  allows  control  of  rapidly  varying  frequencies. 
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I. 


INTRODUCTION 


A.  MOTIVATION 

Isolation  of  mechanical  vibration  on  spacecraft  is  becoming  more  of  a  necessity. 
The  use  of  larger  solar  arrays  and  apertures  leads  to  the  presence  of  increased  vibration 
energy  due  to  flexible  modes  in  the  spacecraft  structure.  At  the  same  time  there  is  an 
inevitable  progression  toward  higher  performance  requirements  for  missions  involving 
imaging  sensors,  interferometers,  and  microgravity  experiments  leading  to  the  need  for 
improved  isolation  of  these  sensitive  payloads. 

Passive  isolation  presents  a  reliable,  low-cost  solution  that  is  effective  for 
attenuating  high  frequency  vibrations.  Passive  techniques  typically  are  not  used  for  low 
frequency  vibration  isolation  since  the  delicate  nature  of  the  resulting  mechanism  is 
unable  to  withstand  the  launch  environment.  The  problem  is  compounded  by  the 
presence  of  low  frequency  modes  that  result  from  the  increased  dimensions  of  modem 
spacecraft  structures.  These  modes  are  excited  by  periodic  disturbances  or  impulsive 
disturbances  such  as  sudden  structural  deformation  due  to  thermal  stress,  attitude  control 
thrusters,  etc.  Sources  of  periodic  excitation  include  solar  array  drive  motors,  cryo 
pumps,  and  momentum  wheels. 

Active  control  techniques  allow  a  significant  performance  enhancement  over 
passive  methods,  but  require  sensors,  actuators,  and  processing  equipment  which  must  all 
be  light  weight  and  energy  efficient.  As  space  qualified  sensors,  actuators,  and 
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processors  become  more  capable  and  affordable  active  control  will  become  the  logical 
choice  for  achieving  performance  goals. 

Control  techniques  for  rejection  of  disturbances  are  numerous  and  include 
classical  feedback,  modem  feedback,  disturbance  accommodating  control,  disturbance 
observers,  repetitive  control,  adaptive  control,  adaptive  inverse  control,  adaptive 
feedforward  control,  and  neural  networks  [Ref.  1].  These  methods  require  some  degree 
of  knowledge  of  the  system  dynamics  or  the  disturbances  to  be  controlled.  Adaptive 
techniques  are  best  suited  to  situations  where  the  system  dynamics  and/or  disturbance 
characteristics  are  changing  with  time.  In  most  cases  narrowband  (periodic)  disturbances 
are  most  effectively  controlled  through  the  use  of  feedforward  techniques. 

A  widely  accepted  adaptive  feedforward  method  for  noise  and  vibration  control  is 
the  Filtered-*  LMS  Algorithm  [Ref.  2],  or  the  Multi-Input  Multi-Output  (M3MO) 
implementation  called  the  Multiple  Error  LMS  Algorithm  [Ref.  3].  A  drawback  of  these 
methods  is  that  they  require  a  separately  measured,  disturbance-correlated  reference 
signal  which  is  adaptively  filtered  to  form  the  control  signal.  In  many  cases  a  disturbance 
source  may  be  unanticipated  (an  example  is  the  thermally-induced  solar  array  vibrations 
experienced  by  the  Hubble  Telescope),  and  these  techniques  are  ineffective  against  such 
disturbances  if  the  sensor  placement  does  not  provide  a  strong,  well-correlated  signal. 
LMS-derived  methods  also  require  prior  knowledge  of  the  system’s  dynamics,  which 
may  vary  with  time  due  to  loading  and  environmental  changes,  or  changes  in  orientation 
of  the  solar  arrays  and  communication/sensing  apertures.  Finally,  the  LMS  algorithms  do 
not  provide  the  ability  to  selectively  cancel  disturbances.  This  ability  to  know  which 
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disturbance  frequencies  to  reject  and  which  to  ignore  is  important  because  it  prevents 
actuator  saturation  when  disturbance  frequencies  are  coincident  with,  or  close  to,  system 
modes  that  are  uncontrollable  or  weakly  controllable.  It  also  allows  the  use  of  lighter 
weight,  less  capable  actuators  when  weight  is  a  primary  concern  (e.g.,  in  spacecraft 
applications). 

A  new  technique  called  Clear  Box  approaches  the  control  problem  from  a  system 
identification  perspective  [Ref.s  4,5].  Identification  brings  out  information  that  is 
normally  hidden  or  not  used  in  traditional  "black  box"  disturbance  rejection  methods. 
Requiring  only  the  knowledge  of  actuator  inputs  and  (disturbance-corrupted)  sensor 
outputs,  the  Clear  Box  Algorithm  allows  complete  identification  of  both  the  system 
dynamics  and  the  disturbance  frequencies.  The  identification,  which  can  be  performed  in 
the  presence  of  unknown  periodic  disturbances,  results  in  a  "disturbance-free"  ARX 
(AutoRegressive  with  exogenous  Input)  model  which  is  then  used  to  calculate  a 
"disturbance  effect"  signal.  This  disturbance  effect  signal  and  the  ARX  system  model 
provide  the  information  needed  to  intelligently  identify  and  selectively  cancel 
disturbances  while  taking  into  account  limited  actuation  resources.  All  of  the  required 
information  can  be  extracted  from  identification  alone  without  requiring  measurement  of 
a  separate  disturbance-correlated  signal. 

Thus,  the  shortcomings  of  the  LMS-based  algorithms  are  addressed  while 
allowing  consistent  reduction  of  the  error  to  the  sensor  noise  level.  This  dissertation  also 
implements  a  new  control  version  of  the  Clear  Box  Algorithm  called  the  "Adaptive  Basis 
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Method"  that  is  capable  of  controlling  rapidly  varying  disturbance  frequencies  by  using 
adaptive  basis  functions  to  synthesize  the  control  signal. 

B.  LITERATURE  REVIEW 

1.  Background 

The  earliest  efforts  to  actively  control  sound  (acoustic  vibration)  are  traced  to  Paul 
Lueg  of  Germany  [Ref.  6]  who  attempted  to  control  sound  propagating  in  a  duct  through 
superposition  of  a  180  degree  phase-shifted  sound  wave,  resulting  in  destructive 
interference  at  the  desired  point  in  the  duct.  Problems  with  standing  waves  arose  that 
could  not  be  solved  with  the  technology  available  at  that  time,  and  interest  in  the  subject 
did  not  resurface  until  the  1950's  in  the  United  States.  Olson's  "Electronic  Sound 
Absorber"  [Ref.s  7,8]  successfully  minimized  sound  levels  using  a  microphone  and  a 
canceling  source  (speaker)  in  a  room  environment.  However,  the  effectiveness  of  the 
system  rapidly  diminished  as  the  distance  between  the  canceling  source  and  the  control 
point  (the  microphone)  was  increased.  As  technological  advances  continued,  interest  in 
the  subject  increased.  The  problem  of  controlling  sound  in  a  duct  was  eventually  solved 
by  Jessel  [Ref.  9]  and  Swinbanks  [Ref.  10]  who  eliminated  the  standing  wave  problem 
encountered  by  Lueg  by  employing  multiple  cancellation  sources.  Swinbanks'  work  was 
continued  and  improved  upon  by  Poole  [Ref.s  11,12]  from  1976-1978. 

Up  to  this  point  the  techniques  employed  for  sound  reduction  consisted  mainly  of 
simple  gain  and  phase  adjustments  to  cause  destructive  interference,  and  the  dynamics  of 
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the  system  were  not  used  in  determining  the  canceling  signal.  The  availability  of  better 
processors  in  the  late  1970’s  led  to  the  first  use  of  signal  processing  techniques  to 
actively  control  sound  and  (for  the  first  time)  structural  vibration.  These  new  techniques 
used  an  analytical  model  of  the  physical  system  to  determine  the  appropriate  control 
signal,  with  a  great  deal  of  new  work  appearing  in  the  mid  1980’ s.  Some  notable 
examples  include  control  of  exhaust  noise  [Ref.  13],  spinning  spacecraft  [Ref.  14],  mass¬ 
spring-damper  systems  [Ref.s  15,16],  vibration  isolation  platforms  [Ref.s  17,18],  rotors 
[Ref.s  19,20],  flexible  beams  [Ref.s  21,22],  and  industrial  machinery  [Ref.  23]. 

The  “model  reference”  approaches  used  to  this  point  were  susceptible  to 
performance  degradation  if  the  system  parameters  changed  due  to  environmental,  load,  or 
structural  changes.  System  identification  techniques  were  still  being  refined,  and  many 
physical  systems  were  too  complex  to  model  accurately  using  analytical  methods.  This 
led  to  an  increasing  interest  in  adaptive  control  which  had  attained  a  solid  foundation  by 
the  late  1970’s  [Ref.  24], 

2.  Adaptive  Control 

Adaptive  control  allows  optimal  performance  in  the  presence  of  modeling  errors 
or  changing  system  parameters  since  the  control  parameters  adapt  to  minimize  an  error 
signal,  and  are  not  strictly  dependent  on  the  model  of  the  system  being  controlled. 
Examples  of  adaptive  control  algorithms  that  emerged  from  early  work  include  Least 
Mean  Squares  (LMS)  [Ref.s  25,26,27],  Recursive  Least  Mean  Squares  (RLMS)  [Ref. 
28],  and  the  Adaptive  Lattice  Filter  [Ref.s  29,30].  The  LMS  Algorithm  (Widrow,  et.  al., 


5 


1975)  employs  a  Finite  Impulse  Response  (FIR)  filter  to  generate  a  control  signal  from  a 
reference  input.  The  RLMS  Algorithm  (Feintuch,  1976)  employs  an  Infinite  Impulse 
Response  (DR)  filter  to  accomplish  the  same  result,  but  has  met  with  resistance  due  to  an 
inability  to  prove  convergence  [Ref.  31]. 

The  adaptive  nature  of  these  controllers  creates  an  inherently  nonlinear  control 
system,  which  results  in  the  stability  and  convergence  properties  being  more  difficult  to 
analyze  than  linear  control  systems  [Ref.  32].  Proofs  of  stability  of  the  LMS  Algorithm 
have  so  far  been  restricted  to  linearized  systems  operating  under  the  restricted  condition 
of  slow  adaptation  [Ref.s  33,34,35,36].  The  advantage  gained  by  the  adaptive  techniques 
is  that  small  errors  in  the  system  model  are  compensated  for  by  the  adaptive  controller. 
However,  larger  changes  in  the  system  dynamics  require  periodic  re-identification  in 
order  to  maintain  optimal  performance  and  stability. 

3.  System  Identification 

There  are  many  methods  available  for  identification  of  system  dynamics  [Ref.  37, 
38,39,40].  The  techniques  include  both  batch  (off-line)  or  recursive  (on-line),  parametric 
or  non-parametric,  and  time  domain  or  frequency  domain.  While  the  presence  of 
unknown  disturbances  or  noise  is  assumed,  these  are  generally  assumed  to  be  white  noise 
or  random  disturbances.  It  is  shown  in  Ref.  41  that  models  identified  in  the  presence  of 
these  periodic  disturbances  can  have  serious  defects  that  render  them  unusable  for  control 
applications.  The  Clear  Box  Algorithm  presented  in  Chapter  II  and  implemented  on  the 
UQP  effectively  removes  the  model  defects  associated  with  unknown  periodic 


6 


disturbances  that  are  present  during  the  identification  process,  and  allows  implementation 
of  an  adaptive  feedforward  controller  based  on  this  disturbance-free  system  model. 

4.  Application  of  Vibration  Control  to  Spacecraft 

Spacecraft  applications  for  vibration  control  are  numerous  [Ref.s  42,43,44], 
including  launch  load  alleviation  [Ref.s  45,46,47],  isolating  the  effects  of  noise- 
producing  equipment  [Ref.s  48,49,50,51,52,53],  isolation  of  sensitive  optics  [Ref.s 
54,55,56,57,58,59,60,61],  and  microgravity  experiment  isolation  [Ref.s  62,63,64,65, 
66,67,68,69,70,71,72,73].  Hexapod  (or  “Stewart”)  platform  [Ref.  74]  configurations 
similar  to  that  employed  in  this  research  allow  control  of  vibration  in  six  degrees  of 
freedom  using  only  linear  actuators  [Ref.s  75,76,77,78,79,80], 

In  general,  there  are  three  types  of  on-orbit  spacecraft  control  implementations 
including  1)  isolation  of  a  noise  source  from  the  structure,  2)  direct  control  of  the  flexible 
structural  members,  and  3)  isolation  of  a  sensitive  payload  article.  Adaptive  control 
methods  are  appropriate  for  all  three  types  of  control  implementations.  The  UQP 
apparatus  employed  in  this  research  is  configured  for  sensitive  payload  isolation,  but 
could  be  adapted  for  noise  source  isolation  as  well. 


C.  THESIS  OVERVIEW 

Chapter  II  presents  a  detailed  outline  of  the  basic  theory  that  supports  the  control 
algorithms  used  in  this  dissertation.  After  reviewing  the  Least  Mean  Squares  (LMS) 
algorithm,  and  its  evolution  into  the  Filtered-x  LMS  (FXLMS)  algorithm  for  vibration 
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and  noise  control,  the  FXLMS  LMS  algorithm  is  expanded  to  a  form  for  multi-input, 
multi-output  (M3MO)  systems  called  the  Multiple  Error  LMS  algorithm. 

Following  a  theoretical  summary  of  the  LMS  algorithm,  the  new  “Clear  Box” 
algorithm  is  presented,  including  a  summary  of  two  distinct  approaches  to  formulating 
control  signals.  The  first  is  the  original  “Sine/Cosine  Method”  where  estimates  of  the 
disturbance  frequencies  are  used  to  generate  a  control  signal  made  up  of  the  combination 
of  sine  &  cosine  pairs.  The  second  “Adaptive  Basis  Method”,  developed  during  the 
course  of  this  research,  generates  a  control  signal  based  on  the  disturbance  effect  r]  (and 
multiple  time-shifted  versions  of  it),  and  does  not  require  disturbance  frequency 
estimates. 

Chapter  El  is  an  overview  of  the  experimental  setup  used  for  the  vibration 
isolation  experiments.  A  description  is  given  of  the  Ultra  Quiet  Platform  (UQP),  and  its 
actuators  and  sensors.  Also  described  are  the  support  electronics  including  the  data 
collection  and  processing  equipment. 

Chapter  IV  Describes  the  system  identification  experiments  performed  on  the 
UQP,  and  describes  the  process  used  to  build  and  verify  a  reference  model  of  the  system 
dynamics.  This  model  is  then  compared  with  a  model  obtained  using  the  Clear  Box 
algorithm  in  order  to  verify  the  accuracy  of  the  results. 

Chapter  V  presents  a  summary  of  the  disturbance  rejection  experiments  that  were 
performed  on  the  UQP.  The  first  experiments  in  each  section  use  the  Multiple  Error 
LMS  algorithm,  followed  by  experiments  using  the  two  methods  of  the  Clear  Box 
algorithm.  The  experiments  include  a  variety  of  narrowband  disturbances,  including 
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single  and  multiple  frequencies  (and  harmonics),  and  also  disturbances  that  are  either 
constant  or  time-varying  in  frequency  and  amplitude.  Also  demonstrated  is  the  ability  of 
the  Clear  Box  algorithms  to  handle  the  case  where  a  disturbance  frequency  coincides 
with  that  of  an  uncontrollable  mode  of  the  system.  The  performance  of  each  algorithm, 
and  the  conclusions  to  be  drawn  from  the  experiments  are  discussed  in  Chapter  VI. 

Chapter  VII  presents  a  summary  of  the  results  and  conclusions.  Also  included  is 
a  description  of  the  unique  contributions  of  this  research,  and  recommendations  for 
further  work. 
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II.  REVIEW  OF  THEORY 


A.  INTRODUCTION 

In  Chapter  I  the  advantages  and  disadvantages  of  several  control  methods  were 
discussed.  From  the  literature,  the  Filtered-x  LMS  (FXLMS)  algorithm  appears  to  be  the 
accepted  choice  for  active  sound  and  vibration  control  due  to  its  simplicity  and  ease  of 
implementation.  However,  the  Clear  Box  algorithm  approaches  the  problem  from  a 
system  identification  perspective,  and  as  a  result  the  algorithm  operates  in  an 
information-rich  environment.  This  added  information  allows  a  more  intelligent 
approach  to  the  disturbance  rejection  problem. 

This  chapter  provides  a  review  of  the  mathematical  formulation  of  the  FXLMS 
algorithm  and  the  extension  to  the  M3MO  version  called  Multiple  Error  LMS.  Following 
this  review,  an  outline  of  the  Clear  Box  identification  and  control  formulation  follows. 
These  formulations  are  the  basis  for  control  experiments  performed  Chapter  V. 


B.  LMS  ALGORITHM 

Since  the  FXLMS  algorithm  is  derived  from  the  Least  Mean  Squares  (LMS) 
algorithm,  we  first  present  the  LMS  adaptive  algorithm  in  order  to  introduce  the  features 
of  FXLMS. 
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1.  Formulation  of  the  LMS  Algorithm 

The  goal  of  the  LMS  algorithm  is  to  use  an  nth  order  digital  HR  filter  W  to 
generate  a  feedforward  control  signal  y(k)  and  minimize  the  mean  square  error  of  e(k) , 
which  represents  the  difference  between  y(k)  and  the  disturbance  signal  d(k)  (shown  in 
Figure  II-l).  This  mean  square  error  will,  from  this  point  on,  be  referred  to  as  %(k) .  The 
algorithm  requires  a  “reference  signal”  x{k)  that  is  correlated  with  the  disturbance  signal 
d(k)  in  order  for  the  controller  to  perform  well,  as  will  be  shown  later. 


Figure  II-l:  LMS  Filter 


Each  choice  of  the  n+1  filter  coefficients  in  W  yields  a  different  E,(k) ,  leading  to 
an  n+ 2  dimensional  performance  surface.  For  example,  a  first  order  filter  would  have 
two  coefficients  ( w0  and  w1 ).  Plotting  the  £(k)  with  respect  to  these  coefficients  would 
result  in  the  three-dimensional  “performance  surface”  shown  in  Figure  II-2. 
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Figure  II-2:  Performance  Surface 
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The  LMS  algorithm  attempts  to  minimize  %(k)  by  “directing”  the  filter 
coefficients  toward  the  minimum  point  on  the  performance  surface.  In  general,  gradient 
descent  methods  converge  to  a  local  minimum,  unless  the  expression  for  |  (k)  is  convex 

in  W  in  which  case  the  local  minimum  is  the  global  minimum.  Referring  again  to 
Figure  II- 1,  e(k)  can  be  expressed  as 


where 


e(k)  =  d(k)  +  X(k)TW 


X(k)  = 


x(k ) 
x(A:-l) 

x(k-n) 


(2-1) 


(2-2) 
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The  mean  square  error  %(k)  of  e(k)  is  defined  as 


t(k)  =  E[e(kf]  (2-3) 


and  it  can  be  expressed  in  terms  of  the  filter  coefficients  since 


eikf  =  d(k)2  +WTX(k)X(k)TW  +  2  d(k)X(k)TW  (2-4) 


Applying  the  expectation  operator  to  each  term  in  (2-4)  gives 


E[£(k)2]  =  E[d(k)2]+WTE[X(k)X(k)T]W  +  2E[d(k)X(k)T]w  (2-5) 


or,  in  matrix  form 

£(k)  =E[d(k)2~\+WTRW  +  2PTW  (2-6) 


where  we  define 


R  =  E[x(k)X(k)T~\, 
P  =  E[d(k)X(k)~\ 
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In  this  expression,  R  represents  the  input  correlation  matrix,  and  P  represents  the  cross¬ 
correlation  between  d(k)  and  the  vector  of  delayed  inputs,  X(k) .  Note  that  there  is  a 

unique  solution  to  the  vector  of  filter  weights,  W ,  if  R  is  positive  definite,  which  occurs 
when 


n  =  2f 


(2-8) 


and/is  the  number  of  frequency  components  in  d(k )  [Ref.  81,82]. 

To  converge  toward  the  global  minimum  of  the  performance  surface  each  filter 
weight  is  updated  along  the  gradient  of  the  surface,  given  by 


A 

v= 


K 

dw 


K 

3w0 


i!_  ii. 

dwl  dwL 


=  2RW  +  2P 


(2-9) 


The  minimum  error  occurs  at  the  global  minimum  point,  where  V  =  0,  and  W  =  W  (the 
*  indicates  the  optimum  solution).  From  Eq.  (2-9)  we  now  have 


0  =  2RW*  +  2P 


(2-10) 


which  results  in  the  matrix  form  of  the  Weiner-Hopf  equation  [Ref.  83] 

W*=-R'lP  (2-11) 
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Using  this  solution  to  determine  an  expression  for  the  minimum  mean  square  error  of 
e(k)  from  Eq.  (2-6)  we  obtain 

^=E[d(kf]+W,TRW‘-HPTW" 

=  E[rf(fc)2]+[/r'pJ  KR-'P-2PrR-'P 

£*  =E[d(kf]-FW’ 

To  update  the  filter  weights,  we  use 

W(k + 1)  =  W(k)  +  ju(-V(£))  (2-13) 

where  jj.  is  the  “adaptation  rate”,  which  is  a  positive  value.  The  exact  computation  of  the 
gradient  at  time  step  k,  V(&) ,  is  inefficient  to  calculate.  The  simplicity  of  the  LMS 
algorithm  comes  from  employing  an  estimate  of  the  gradient.  Since  from  Eq.  (2-3)  we 
have  £(&)  =  £[e(fc)2],  we  can  simply  remove  the  expectation  operator  to  obtain  an 

estimate  of  %(k) .  Now  we  use  £(&)  =  £( k)2,  and  recall  that  e(k)  =  d(k)  +  X(k)TW  to 
obtain  the  estimate  of  the  gradient  at  step  k 
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<> 

3 

5(e(k)2)" 

=  2e(k) 

’de(k)" 

=  2e(k)X 

(2-14) 

d(£(k)J) 

dwL 

deCk) 

Now  the  update  to  the  filter  weights  is  accomplished  at  each  time  step  using  the  measured 
error  and  the  reference  input  as  follows 

W(k+l)=W(k)-2ne(k)X  (2-15) 

The  maximum  adaptation  rate  that  can  be  used  (without  causing  instability)  is  given  by 

0</i<— (2-16) 
'W 

where  Aniax  is  the  largest  eigenvalue  of  the  input  correlation  matrix  R  [Ref.  84].  An 
alternative  upper  bound  is  l/tr[/f],  which  is  more  restrictive  but  easier  to  calculate  [Ref. 
85].  The  resulting  convergence  of  the  filter  coefficients  W  achieves  a  global  minimum 
mean  square  error  £(&) . 
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2.  Formulation  of  the  Filtered-*  LMS  Algorithm 

Note  that  the  structure  of  the  problem  outlined  for  the  LMS  algorithm,  above, 
differs  from  the  disturbance  rejection  problem  of  interest  here.  Figure  EE-3  shows  that 
there  is  a  physical  plant  between  the  output  of  the  filter  W  and  the  signal  e(k) . 


Model  of  P2  r(k) 


Figure  II-3:  Filtered-*  LMS  System  Representation 


The  disturbance  source  acts  on  the  physical  system  through  the  plant  Px  (with 
unknown  dynamics)  referred  to  as  the  “primary  plant”.  A  sensor  provides  a  disturbance- 
correlated  reference  signal  x(k) .  The  Filtered-*  LMS  algorithm  determines  an 
appropriate  feedforward  control  signal  g(k)  that  acts  on  the  system  through  the 
“secondary  plant”  P2 ,  which  in  this  case  is  the  UQP’s  active  strut  system  (see  Chapter  in 
for  a  complete  description  of  the  UQP  experiment  apparatus).  The  error  at  the  system 
output  e(k)  is  the  result  of  the  actions  of  the  disturbance  source  and  g(k)  on  the  primary 
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and  secondary  plants,  respectively.  The  existence  of  the  plant  P2  between  the  filter  and 
e(k)  requires  a  change  in  the  way  the  LMS  algorithm  is  applied. 

The  key  is  to  start  with  a  form  of  the  problem  that  minimizes  the  mean  square 
error  of  e(k)  at  the  output  of  the  filter  W,  as  shown  in  Figure  II-4  a).  Next  we  simply 
duplicate  the  P2  box  for  an  equivalent  representation  in  Figure  II-4  b).  Finally  we 
assume  that  the  filter  W  is  changing  very  slowly  with  time.  We  make  this  assumption  so 
that  the  property  of  commutability  between  W  and  P2  holds  (i.e.  WP2  =  P2W ),  and  thus 
make  the  final  transition  to  Figure  H-4  c).  Adding  the  primary  plant  path  results  in  the 
final  configuration  of  the  disturbance  rejection  problem  originally  shown  in  Figure  II-3. 

The  reason  for  the  “Filtered-*”  designation  of  the  algorithm  comes  from  the  fact 
that  the  reference  input  x(k)  is  filtered  by  a  model  of  the  secondary  plant  before  being 
used  in  the  LMS  algorithm  to  update  the  filter  weights.  Thus,  the  only  difference 
between  the  equations  used  to  implement  the  LMS  and  Filtered-*  LMS  algorithms  is  that 
the  term  X  used  in  Eq.  (2-15)  consists  of  a  filtered  version  of  the  original  signal. 
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Figure  II-4:  Transformation  to  Filtered-*  LMS 

3.  Extension  to  the  Multiple  Error  LMS  Algorithm 

The  UQP  is  configured  similarly  to  a  Stewart  Platform  [Ref.  86],  which  employs 
orthogonal  struts  to  minimize  coupling  between  a  given  strut’s  actuator  and  neighboring 
sensors  on  other  struts.  Under  perfectly  decoupled  conditions  the  UQP  could  employ  six 
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single-input-single-output  (SISO)  Filtered-*  LMS  controllers  with  no  loss  of 
performance.  However,  coupling  does  exist  between  struts  (as  will  be  shown  in  Chapter 
IV),  and  better  performance  results  if  a  multi-input  multi-output  (MIMO)  controller  is 
used. 

The  Filtered-*  LMS  algorithm  has  been  extended  to  a  MIMO  version  [Ref.  87] 
called  the  Multiple  Error  LMS  algorithm.  For  the  development  of  this  algorithm  it  is 
assumed  that  there  are  M  actuators,  and  L  sensors.  Again,  there  is  a  reference  signal 
x(k )  which  passes  through  a  “primary  plant”  before  being  sensed  at  the  system  output 

(Figure  II-l)  as  d(k) .  The  disturbance  at  the  Zth  sensor  is  represented  by  cll  (k) . 


Zth  Order  r(k) 

FIR  Filter 
(Model  of  P2) 


Figure  II-5:  Multiple  Error  LMS  Algorithm 


The  plant  model  used  to  filter  the  reference  signal  is  a  7th  order  Finite  Impulse 
Response  (FIR)  filter  C  whose  coefficients  clmj  indicate  the  yth  coefficient  ( j  =  1, . . . ,  J ) 
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for  the  filter  that  models  the  dynamics  between  the  mth  actuator  and  the  Ith  sensor.  The 
resulting  filtered  signal  r(k)  includes  LxM  elements  similarly  indicated  by  rlm(k) .  The 
M  control  signals  in  g{k)  are  generated  by  filtering  the  reference  signal  with  an  7th  order 
FIR  filter  W  whose  coefficients  are  wmj .  Finally,  the  error  signal  at  each  of  the  L  sensors 
is  indicated  by  et(k) ,  an  expression  for  which  is 

M  7-1  / 

£i  (*)  =  d,  (k) +  £  £  clmj  £  wmi  in  -  j)x(n  -  i  -  j )  (2-17) 

m- 1  j- 0  i=0 

As  long  as  each  dt  ik)  is  partially  correlated  with  x(k)  it  is  possible  to  reduce  the 
error  at  each  sensor  through  the  proper  choice  of  the  coefficients  wmj .  By  defining  the 
total  error  as 


J  =  E\Jje?(k)\ 


/= i 


(2-18) 


from  Eq.s  (2-17)  and  (2-18)  it  is  clear  that  7  is  a  quadratic  function  of  each  of  the 
coefficients  wmi ,  indicating  that  gradient  descent  methods  allow  convergence  to  a  global 
minimum  J.  The  differential  of  J  with  respect  to  one  coefficient  is 


dJ 

dWrru 


=  2E  5>,(fc) 


/=1 


de^k) 

dw. 


(2-19) 
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Partially  differentiating  Eq.  (2-17)  with  respect  to  wmi  we  obtain 


det(k) 

dwmi 


J-l 


;=0 


(2-20) 


The  above  quantity  is  the  same  as  that  obtained  by  filtering  the  reference  signal,  delayed 
by  i  samples,  with  the  FIR  filter  C,  which  is  denoted  by  rlm  ( k  -  i ) .  Thus  we  have 


de ,(£) 

dwml 


nJk-i) 


(2-21) 


Adjusting  each  filter  coefficient  in  IT  by  the  negative  of  the  gradient  expression  in 
Eq.  (2-19),  and  using  the  expression  in  Eq.  (2-21),  we  obtain 


wmi(k + 1)  =  wmi  ( k )  -  2^  ei  ( k)rlm  (k  -  i )  (2-22) 

1=1 


where  fi  is,  once  again,  the  adaptation  rate.  When  L  =  M  =1  this  algorithm  reduces  to 
the  result  obtained  in  Eq.  (2-15)  for  the  LMS  and  FXLMS  algorithms. 
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The  assumption  of  time  invariance  in  the  wmi  filter  coefficients  is  equivalent,  in 

practice,  to  assuming  that  the  filter  coefficients  change  only  slowly  compared  to  the 
timescale  of  the  response  of  the  system  to  be  controlled  [Ref.  88]. 

4.  Stability 

Proofs  of  stability  of  the  LMS  Algorithm  have  so  far  been  restricted  to  linearized 
systems  operating  under  the  restricted  condition  of  slow  adaptation  [Ref.s  89,90,91,92, 
93],  and  are  typically  limited  to  analysis  of  the  single-input  single-output  case,  although 
recent  proofs  have  addressed  the  MEMO  case  [Ref.  94,95], 

To  maintain  stability  the  adaptation  rate  }X  for  the  SISO  Filtered-*  LMS 
Algorithm  must  be  chosen  less  than  the  upper  bound  set  in  Eq.  (2-16).  A  recent  text  by 
Fuller  et.  al.  [Ref.  96]  offers  a  guideline  for  the  Multiple  Error  LMS  Algorithm 
adaptation  rate 


0  <{x< 


1 

2  r2l 


(2-23) 


where'  r 2  is  the  mean  square  level  of  the  filtered  reference  signal  r(k) ,  and  I  is  the  order 
of  the  adaptive  filter. 
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C.  CLEAR  BOX  ALGORITHM 


1.  System  Representation 

We  approach  the  identification  problem  by  assuming  that  the  system  can  be 
represented  by  a  linear  discrete-time  state  space  model  of  the  form 

x(k  +  l)  =  Ax(k)  +  Bu(k)  +  Bdd(k) 
y(k)  =  Cx(k) 

where  x(k)  is  an  nxl  state  vector,  u{k)  is  an  mxl  input  vector,  and  y(k)  is  a  qxl  output 
vector.  Similarly  the  system  A,  B,  and  C  matrices  have  dimensions  nxn,  nxm,  and 
qxn ,  respectively.  The  system  is  represented  in  Figure  II-6.  It  is  assumed  that  nothing 
is  known  except  for  the  recorded  system  input  u(k),  the  disturbance-corrupted  output  data 
measurements  y(k),  an  upper  bound  on  the  true  system  order  n,  and  an  upper  bound  on 
the  number  of  disturbance  frequencies/. 


Figure  II-6:  System  Representation  for  Clear  Box  Algorithm 
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2. 


/7-Step  Ahead  Predictions 


Eq.  (2-24)  is  a  one-step-ahead  prediction  of  the  state,  based  on  the  state  and 
system  input  at  the  current  time.  By  a  recursive  procedure  we  can  determine  the 
equations  for  a  p-step-ahead  predictor  given  by 


x(k  +  p)  =  Apx(k ) + C  up(k)+C  ddp  ( k )  (2-25) 

where  up(k)  and  dp(k)  are  vectors  of  the  control  inputs  and  disturbances, 


u(k ) 

d(k) 

up(k)  = 

u(k  +1) 

,  dp(k)  = 

d(k+ 1) 

u{k  +  p- 1) 

/< — s 

+ 

1 

h— ^ 

The  matrices  c  and  q  are  of  a  form  similar  to  the  controllability  matrices  associated  with 
the  control  input  and  disturbance  excitation,  respectively,  and  are  given  by 

C  =[Ap-xB,...,AB,B],  Cd  =  [Ap~lBd, ABd,Bd]  (2-27) 

The  output  equation  can  similarly  be  propagated  forward  in  time,  with  the  result 

yp  (k)  =Ox(k)  +  Tup(k)  +  Tddp(k)  (2-28) 
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where  the  elements  of  the  equation  are  defined  by 


c 

yP(k)  = 

m 

y(k+ 1) 

CA 

,  0  = 

CAP~2 

_y(k  +  p- 1)_ 

CAp~l 

(2-29) 


1 

o 

o 

o 
_ 1 

1 

o 

o 

o 

o 

8 

cb,  o  •.  : 

CAB  CB  \  : 

.  Td  = 

CABd  CBd  ••.  ; 

:  \  \  0 

i  \  0 

CAP'2B  •••  CAB  CB  0 

CAp-2Bd  •••  CABd  CBd  0 

o  is  a  pqxn  matrix  similar  in  form  to  the  system  observability  matrix,  t  is  a  pqxpm 
Toeplitz  matrix  with  its  elements  corresponding  to  the  qxm  system  Markov  parameters 
CB,  CAB,  ...  ,  CAp2B,  whose  elements  are  the  system  response  to  a  unit  pulse  applied  at 
each  control  input. 

3.  Removing  Dependence  on  the  Initial  State  and  the  Disturbance  Time 
History 

Equations  (2-25)  and  (2-28)  represent  the  system  input-output  mapping,  and  at 
this  point  their  solution  depends  on  the  initial  state  x(k)  and  the  disturbances  dp(k).  The 
goal  is  for  the  algorithm  to  rely  solely  on  the  input  and  output  time  histories  (up(k)  and 
yp(k)).  Note  that  the  system  input  signal  up(k)  must  have  sufficiently  rich  frequency 
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content  to  excite  all  of  the  modes  of  the  system.  Application  of  band-limited  white  noise 
to  the  system  can  satisfy  this  condition. 

To  eliminate  Eq.  (2-25)’ s  dependence  on  x(k)  and  dp(k)  additional  degrees  of 
freedom  are  introduced  into  the  model  by  adding  and  subtracting  Myp(k)  to  the  right  side, 
resulting  in 


x(k  +  p)  =  Apx(k) +Cup(k)+C  ddp  ( k ) + Myp  (i k )  -  Myp  ( k )  (2-30) 


where  M  is  arbitrary,  and  of  dimension  nxpq.  Substitution  for  yp(k)  (from  Eq.  (2-28)) 
into  the  above  equation  yields 


x(k  +  p)  =  Ap  x(k) +Cup(k)  +  C  ddp  (k)  +  M{ox(k)  +  Tup(k)  +  Tddp(k))-  Myp  (k) 
=  (Ap+MO  )x(£)  +  (c  +MT  )up(k)  +  (cd+Mld)dp(k)-Myp(k ) 


(2-31) 


In  order  to  eliminate  x(k)  and  dp(k)  from  Eq.  (2-31),  M  must  be  chosen  to  satisfy 
the  following  two  conditions  (for  all  values  of  k ), 


Ap+M  O  =0 


(2-32) 


(cd+MTd)dp(k)  =  0  Vk 


(2-33) 


so  that  Eq.  (2-31)  becomes 
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x(k  +  p)  =  (c  +M T  )up{k)-Myp(k ) 


(2-34) 


Equation  (2-33)  represents  a  set  of  constraints  that  must  be  satisfied  for  all  values  of 
k=l,  2,...,  N,  N+ 1, . . . .  These  constraints  can  be  grouped  together  such  that 


(cd+MTd)D  =0 


(2-35) 


where 


D  =[dp{\\  dp{ 2),...  dpm  </,(*  +  !),...] 


(2-36) 


Since  the  rows  of  the  matrix  d  are  made  up  of  the  time-shifted  histories  of  the 
disturbance  signal,  and  assuming  that  there  are  /  distinct  frequencies  present  in  the 
disturbance  signal,  there  is  a  limit  to  the  possible  rank  of  d  even  if  the  available  time 
history  is  infinite.  The  maximum  rank,  p,  of  d  is  equal  to  2/ ,  or  2/  +1  if  any  of  the 
disturbances  has  non-zero  mean.  The  result  of  this  observation  is  that  the  maximum 
number  of  constraints  represented  by  Eq.  (2-35)  is  np  (where  n  is  the  true  order  of  the 
system).  In  order  to  show  that  a  solution  M  exists  that  meets  all  of  the  required 
constraints,  we  will  use  the  following  arguments. 
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Let  D/  be  formed  from  p  linearly  independent  columns  of  d,  so  that  it  is  now 
pxp.  Similarly  the  dimensions  of  rd  and  cd  can  be  reduced  to  pqxp  and  nxp, 
respectively.  Combining  Eq.s  (2-32)  and  (2-35),  the  equations  that  M  must  satisfy  are 


M[0,TdBf]  =  -[Ap,  cpf] 


(2-37) 


where,  again,  M  is  nxpq,  o  is  pqxn,  and  Ap  is  nxn.  From  the  dimensions  of  the 
matrices  we  see  that  Eq.  (2-37)  represents  n2+np  linear  equations  in  nxpq  =  npq 
unknowns  in  M.  A  solution  for  M  exists  if  [o  ,  TJD  f  ]  is  full  (column)  rank,  and  p  is 

chosen  such  that  nqp  >n2  +  pn.  Recalling  that  p  =  2/  + 1 ,  this  condition  for  the 
existence  of  M  can  be  expressed  as 


P>^1± 1 
q 


(2-38) 


Thus,  if  an  upper  bound  on  the  true  system  order  n  and  the  maximum  number  of 
frequencies  that  need  to  be  controlled  are  known,  p  can  be  chosen  such  that  a  solution  M 
exists.  [Ref.  97] 
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4.  Disturbance-Corrupted  System  Model 

The  condition  expressed  in  Eq.  (2-38)  assures  that  the  p-step  ahead  state 
prediction  in  Eq.  (2-34)  is  valid.  Pre-multiplying  Eq.  (2-34)  by  C  results  in  an  input- 
output  model  of  the  form 

y(k  +  p)  =  C(c  +MT  )up(k)-CMyp(k )  (2-39) 

Shifting  the  time  index  back  by  p  steps  gives,  for  k  >  p 

y(k )  =  C  (C  +  MT  )up (k  -  p)  -  CMyp (k  -  p )  (2-40) 


where 


u(k  -  p) 

"  y(k-P)  ' 

up(k-p)  = 

u(k-  p+ 1) 

il 

1 

w 

y(k-p  + 1) 

u(k- 1) 

y(*-D 

So  we  now  have  a  model  that  predicts  the  next  system  output  condition,  given  the  last  p 
sets  of  disturbance-corrupted  input-output  data.  The  input-output  model  in  Eq.  (2-40)  has 
the  same  form  as 
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y(k)  =  aj(k  - 1)  +  a2y(k  -2  )  +  —  +  apy(k-p) 
+  fixu{k  - 1)  +  j 62u(k  -  2)  +  •  •  •  +  fipu(k  -  p ) 


(2-41) 


where  ava2,...,ap  and  A>  P2,...,(3p  are  the  model  coefficients.  These  coefficients  are 
related  to  the  matrices  in  Eq.  (2-40)  as  follows 


[ap,ap_1,...,a1]  =  -CM,  [(3p,  ..,&]  =  C(C  +  MT )  (2-42) 

Note  that  since  this  model  is  derived  from  disturbance-corrupted  data  it  will  include 
modes  that  are  not  part  of  the  system  dynamics.  Assuming  the  disturbances  are 
narrowband  (sinusoidal)  in  nature,  the  extraneous  modes  in  Eq.  (2-41)  will  have 
frequencies  corresponding  to  those  of  the  disturbances. 

5.  Disturbance-Free  System  Model 

The  identification  of  the  system  model  may  need  to  be  accomplished  under 
various  conditions,  depending  on  the  application  and  the  quality  of  the  sensor  data 
available.  The  system’s  dynamic  model  may  be  relatively  constant,  or  may  be  rapidly 
time  varying.  For  the  UQP  application,  the  dynamics  of  the  system  (from  actuator  input 
to  sensor  output)  are  fairly  constant  over  time,  and  thus  the  identification  can  be 
accomplished  in  batch  mode  a  single  time,  or  periodically  if  conditions  warrant  (if  a 
malfunction  occurs,  or  adjustments  are  made,  etc.)  If  the  system  model  is  known  to 
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change  with  time,  then  the  identification  can  be  accomplished  recursively  so  that  constant 
updates  are  available. 

a)  Identification  from  Disturbance  Corrupted  Data 

Assuming  that  the  system  identification  must  be  done  in  the  presence  of 
disturbances  that  cannot  be  “turned  off’,  the  available  input-output  data  will  be  corrupted 
by  disturbance  effects  that  are  not  part  of  the  true  system  dynamics.  The  assumption  is 
also  made  that  the  sensor  data  is  contaminated  with  some  degree  of  noise.  In  this  case  a 
least  squares  solution  of  the  ARX  model  coefficients  is  warranted,  and  can  be  found 
using 


[C(C  +MT  ),  -CM]  =  YVT  (yVTJ  (2-43) 

where  the  Y  and  V  matrices  are  formed  from  input-output  data  as  follows. 


Y  =  [y(p),  y(/?+l),...,y(/)], 


«P(0)  up(  1)  •••  up(l-p ) 

yP(0)  yp(f)  •••  ypQ-p) 


(2-44) 


with  up  ( k )  and  yp  (£)  as  in  Eq.  (2-26).  The  a,  and  j8(.  coefficients  are  found  from  Eq.s 

(2-42)  and  (2-43).  For  an  ARX  model  of  order  p  to  be  generated,  at  least  p  data  samples 
must  be  available.  Since  noise  is  assumed  to  be  present,  the  use  of  substantially  more 
data  points  will  have  the  affect  of  averaging  and  will  generally  give  a  better  result. 
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When  using  noisy  data  it  is  generally  useful  to  choose  p  large  enough  to 
include  all  of  the  disturbance  modes  plus  a  number  of  “noise  modes”  or  over¬ 
parameterization  modes.  At  this  point  the  identified  system  model  coefficients  will 
include  three  types  of  modes;  true  system  modes,  disturbance  modes,  and  noise  modes. 
The  disturbance  modes  need  to  be  removed  in  order  to  obtain  the  disturbance-free  model. 
This  process  is  described  in  the  next  two  sections. 


b )  Disturbance  Identification  through  Modal  Decomposition 

To  facilitate  the  removal  of  the  disturbance  modes  it  is  convenient  to 


convert  the  ARX  model  to  an  equivalent  state  space  observable  canonical  form 


where 


Uk  +  l  )  =  Apz(k)  +  BpU(k) 
{  y(k)  =  Cpz(k) 


a,  7  0  •••  0 

'A' 

a2  0  7  • .  i 

A 

a3  0  0  \  0 

;  •  :  \  7 

.  Bp  = 

A 

,  Cp  =[7  0  0  -  0]  (2-46) 

a p  0  0  •  •  •  0 

A. 

Conversion  of  this  model  to  modal  form  yields  the  state  space  equations 
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w(k + 1)  =  A w(k)  +  Tu(k) 
y(k )  =  £lw(k) 


(2-47) 


where  A,  T,  and  Q.  are  formed  via  similarity  transformation; 


A  =  T-'ApT,  r  =  T~lBp,  Q  =  CpT  (2-48) 


Assuming  Ap  is  diagonalizable,  the  columns  of  the  transformation  matrix,  T,  are  the 
eigenvectors  of  Ap .  Each  oscillatory  mode  of  Ap  results  in  a  pair  of  eigenvectors  that 

are  complex  conjugates  of  each  other.  In  this  case  one  column  of  T  is  formed  from  the 
real  part  of  this  eigenvector  pair,  and  a  second  column  is  taken  from  the  imaginary  part. 
Any  non-oscillatory  (real)  modes  of  the  system  will  result  in  a  single  real  eigenvector 
column  added  to  T  (the  corresponding  real  eigenvalue  is  A(. ).  Building  the  transformation 
matrix  in  this  manner  results  in  the  diagonalized,  or  decoupled  transition  matrix 


A  =  diag 


~  °x 

$ 

_ _ 1 

>  •  •  •  1 

cr 

2 

,  s 

_ 1 

-(0, 

°ij 

-con 

2c_ 

Gn 

2 

2  _ 

„ 

(2-49) 
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where  nr  and  nc  are  the  number  of  real  and  complex  modes,  respectively,  and 
nr+nc  =  pq .  The  complex  eigenvalue  pairs  are  represented  by  the  2x2  block  matrices 

n 

on  the  diagonal  of  A,  the  ith  complex  eigenvalue  pair  is  cr  ±  j(Oi ,  where  i  =  1, 2, . . . ,  -j- . 

£ 

The  output  and  input  influence  matrices  are  given  by 


c(1),c(2) 

>  c. 


(2-50) 


where  c(r° ,  c(c°  and  b(p ,  b(c°  are  the  respective  output  and  input  influence  coefficients 
associated  with  the  real  eigenvalues  Ar(,)  and  the  complex  conjugate  eigenvalue  pairs 
<7(cl)  ±  j0)(cl) .  The  pulse  response  of  the  Ith  real  or  complex  conjugate  mode  is  given  by 


Pra)  (k)  =  cf  (Ar(0  )t_1  bf , 


and  P0)  (k)  =  c 


,  -  -to 


r® 


-0) 


(0 


Ik- 1 


0), 


(0 


b(c°  (2-51) 


and  the  total  system  pulse  response  is 
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(2-52) 


Pt(k)  =  %  Pr0)  (k)  +  5  Pc(i)  (k)  =  %  P0)  (k) 

/=i  i=i  i=i 

Again,  nr  is  the  number  of  real  eigenvalues,  nc  is  the  number  of  complex  conjugate 
pairs,  and  nm  =  nr  +  nc . 

Now  it  is  possible  to  calculate  the  relative  contribution  of  each  mode  to 
the  total  system  pulse  response  by  taking  the  inner  product  of  each  modal  pulse  response 
with  the  total  system  pulse  response  (over  N  samples),  as 

S(i)  =  ^Pt(k)-P0)(k)  (2-53) 

jfc=l 

If  the  Ith  mode’s  pulse  response  is  well  correlated  with  the  total  system 
pulse  response  then  the  elements  of  the  qxm  matrix  S(i)  will  be  relatively  large,  which 
is  an  indication  that  a  mode  is  one  of  the  tme  system  modes,  as  opposed  to  a  noise  mode. 
To  obtain  a  scalar  “modal  pulse  response  norm”  discriminator  for  each  mode  [Ref.  98], 
the  elements  of  S(,)  can  be  summed  so  that 


k=  1  (=1 


(2-54) 
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The  lower  portion  of  Figure  IV-10  on  page  87  shows  an  example  of  how  these  norms  are 
used  to  isolate  the  true  system  modes  from  the  other  modes  present  in  the  model  (noise 
and  disturbance  modes). 

The  periodic  disturbance  modes  are  oscillatory  with  their  z-plane  poles 
being  very  close  to  the  unit  circle  (just  inside  or  just  outside  depending  on  the  particular 
set  of  data  used).  This  results  in  near  zero  (positive  or  negative)  damping  for  the 
disturbance  modes,  which  is  to  be  expected  for  forced  oscillations.  Typically,  if  p  is 
chosen  large  enough,  the  damping  ratios  of  the  disturbance  modes  will  be  at  least  one  or 
two  orders  of  magnitude  smaller  than  those  of  the  noise  modes  or  true  system  modes,  and 
they  are  easily  identified  (see  the  upper  portion  of  Figure  IV-10).  Also,  the  accuracy  of 
the  identified  frequency  improves  as  p  increases.  The  frequencies  of  the  disturbances  are 
thus  readily  determined  when  the  model  is  converted  to  this  modal  form. 


c)  Disturbance  Free  ARX  Model  and  the  Disturbance  Effect 
Once  the  disturbance  modes  have  been  identified  in  the  modal  state  space 

model,  they  can  be  removed  by  eliminating  the  corresponding  rows  and  columns  from  A, 
T,  and  Q.  The  resulting  matrices  are  the  “disturbance-free”  versions,  which  are  denoted 

by  a  “bar”  on  top;  A ,  T ,  and  Q, .  These  are  simply  the  A,  B ,  and  C  matrices  converted 
by  a  similarity  transformation,  and  they  describe  the  same  system.  The  only  difference  is 
that  the  A,  T ,  Q  model  form  uses  a  different  state  vector  w  related  to  x  by  the 
transformation  x{k )  =  Tw{k) .  Since  the  “intermediate  step”  of  calculating  the  state 
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vector  is  not  needed  in  this  problem,  and  in  both  cases  the  same  y(k)  output  is  the  result, 
it  is  equivalent  to  use  “  A  ”,  “  B  ”,  and  “  C  ”  to  describe  the  disturbance-free  model  in  state 
space  form. 

Recall  from  Section  3  that  a  system  input-output  model  in  ARX  form  was 
found  by  using  additional  freedom  introduced  by  the  matrix  M  to  eliminate  the 
dependence  of  the  model  on  the  initial  state  and  the  disturbance  input(s).  The  same 
approach  is  used  here,  but  the  goal  is  only  to  remove  the  dependence  on  the  initial  state 
and  retain  the  “disturbance  effect”  for  later  use. 

The  p-step  ahead  state  prediction  equation  using  the  disturbance-free 
system  parameters  is  thus 


x(k  +  p)  =  ( Ap  +  MO  )x(k) + (c  +  MT  )up ( k )  +  (c  d  +  MTd  )dp ( k ) - Myp ( k )  (2-55) 


The  C  ,  O  ,  and  T  matrices  are  formed  the  same  way  as  in  Eq.s  (2-27)  and  (2-29),  but 
A,  B,  and  C  are  used  in  place  of  A,  B,  and  C.  To  remove  the  dependence  on  the  initial 
state  x(k) ,  the  condition  that  must  be  met  is 

Ap  +MO  =0  (2-56) 

Since  O  is  full  column  rank  (the  identified  system  is  fully  observable),  M  can  be  found 
from 
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*~(W 


(2-57) 


as  long  as  p  is  chosen  so  that  pq>n .  This  choice  for  the  “arbitrary”  matrix  M 
eliminates  the  dependence  on  the  initial  state,  but  leaves  the  disturbance  term  intact.  Pre¬ 
multiplying  Eq.  (2-55)  by  C  gives  the  result 


y(k)  =  -CMyp(k-p)  +  C(c  +MT  )up(k- p)  +  C(c  d  +MTd) dp(k-p)  (2-58) 


where 


u{k-p) 

u{k  -  p) 

\ - 

1 

1 _ 

yP(k-p)  = 

u(k-  p+ 1) 

,  up(k-p)  = 

u(k-  p- 1-1) 

,  dp  (k-p)  = 

d(k-p+ 1) 

u{k  —  1) 

u(k- 1) 

d(k-  Y) 

The  entire  term  in  Eq.  (2-58)  involving  dp(k-p)  (the  last  term  on  the 

right  side)  is  simply  a  linear  combination  of  the  disturbance  inputs  to  the  system,  and 
represents  the  forced  response  of  the  system  to  these  disturbances.  With  this  term 
referred  to  as  r\(k)  Eq.  (2-58)  becomes 

y(k)  =  -CMyp(k- p)  +  c(c  +MT^up(k- p)+rj(k)  (2-59) 
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where  this  is  in  the  form  of  an  ARX  (Auto-Regressive  with  exogenous  input)  model 


y(k)  =  axy{k  - 1)  +  a2y  (k  -  2) + . . .  +  ap  y(k  -  p)  + 

Pxu{k  - 1)  +  p2u(k  -  2)  + . . .  +  fipu(k  -p)+ r](k) 


(2-60) 


and  the  coefficients  are  given  by 

[a,,®,. . ,S,]=-CM,  A  ]  =  C(C+Mf)  (2-61) 

The  models  given  in  Eq.s  (2-41)  and  (2-60)  are  equivalent,  but  two  very 
important  differences  are  that  1)  the  coefficients  are  now  representative  of  the 
disturbance-free  system,  and  2)  the  disturbance  effect  has  been  separated  from  the  system 
dynamics.  Note  that  the  disturbance  effect  separation  was  accomplished  mode-by-mode 
through  column  and  row  elimination.  If  desired,  selected  disturbance  modes  can  be  left 
in  the  system  model,  and  would  remain  absorbed  in  the  a  and  coefficients.  This  is 
desirable  if  a  particular  mode  is  determined  to  be  uncontrollable  or  weakly  controllable. 
This  selectivity  will  be  discussed  further  in  Section  6  below. 

It  is  possible  to  calculate  the  disturbance  effect  directly  by  rearranging  Eq. 
(2-60)  in  the  form 
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r]{k)  =  y(k)-aiy(k-\)-a2y(k-2)-...-apy{k-p) 
-  Pjuik-l) -  j}2u(k  -  2)  - ...  -  Ppu(k  -  p) 


(2-62) 


This  allows  a  real-time  calculation  of  the  disturbance  effect  based  on  the  last  p 
measurements  of  the  input-output  data.  The  calculated  disturbance  effect  in  Eq.  (2-62) 
contains  all  of  the  frequencies  that  were  1)  identified  as  disturbance  modes,  and  2) 
removed  from  the  system  model. 

6.  Control  Formulation 

a )  Choice  of  Control  Signal  Basis  Functions 

At  this  point  the  disturbance-free  system  model  and  the  disturbance  effect 
are  known.  For  a  time-invariant  system,  the  disturbance-free  model  need  only  be  found 
once  through  batch  processing  of  the  input-output  data  and  elimination  of  the  disturbance 
modes.  Using  this  model,  the  disturbance  effect  is  calculated  from  Eq.  (2-62)  in  real-time 
and  is  available  at  each  time  step  (after  p  data  samples  are  taken).  From  Eq.  (2-60),  the 
feedforward  control  uf(k)  that  cancels  the  steady-state  disturbances  must  satisfy 

A«/  (k  - 1)  +  j 32uf  (k  -  2)  + . . .  +  Jipuf  (k  -  p)  =  -tl(k)  (2-63) 

If  the  identified  system  model  is  non-minimum  phase,  the  system  zeros 
present  in  the  f  coefficients  will  cause  the  control  signal  to  grow,  unbounded,  if  an 
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attempt  is  made  to  recursively  calculate  uf(k)  from  Eq.  (2-63).  The  alternative  is  to  take 

advantage  of  the  knowledge  that  the  control  signal  needs  to  be  made  up  of  periodic 
components  to  cancel  the  periodic  disturbances.  Two  options  for  such  an  approach  are 
presented  here  which  exploit  the  fact  that  the  disturbance  effect  signal  contains  all  of  the 
disturbance  frequencies  (assuming  they  were  not  intentionally  retained  in  the  system’s 
ARX  model  coefficients). 

The  first  option  is  to  use  the  sine  function  as  a  basis  for  the  control  signal. 
Analysis  of  the  disturbance  effect  signal  r\{k)  yields  estimates  of  the  disturbance 
frequencies,  and  for  each  frequency  present  a  sine/cosine  “basis  set”  can  be  used  to 
cancel  the  disturbance.  Using  the  frequency  estimates  the  coefficients  for  the  sine  and 
cosine  terms  are  calculated  recursively. 

The  second  option  exploits  the  fact  that  the  exact  disturbance  frequencies 
are  present  in  the  disturbance  effect  signal  rj(k) .  The  time  history  of  rj(k) ,  like  the  sine 
function,  is  a  basis  that  can  be  used  to  generate  the  disturbance-canceling  control  signal. 
The  cosine  function  is  simply  a  time-shifted  sine  function,  and  a  similar  time  shifting  of 
r](k)  will  yield  a  valid  basis  set.  The  attractiveness  of  this  option  is  that  the  disturbance 
frequencies  do  not  need  to  be  estimated  since  the  exact  frequencies  are  already  present  in 

m- 

The  choice  of  which  control  formulation  to  use  depends  mainly  on  the  rate 
of  frequency  variation  expected  in  the  disturbances,  and  somewhat  on  the  available 
processing  power.  All  of  these  considerations  will  be  discussed  in  Chapter  VI,  but  for 
now  the  steps  needed  to  implement  the  two  options  are  discussed  below. 
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b)  Control  Formulation  Using  the  Sine/Cosine  Basis  Set 
The  first  step  in  generating  a  sine/cosine-based  control  signal  is  to 
estimate,  as  accurately  as  possible,  the  frequencies  of  the  disturbances,  and  this  can  be 
done  in  two  different  ways.  The  first  uses  the  system  input-output  data  to  generate  a  full 
system  model  as  described  in  Section  5.b).  The  disturbance  frequencies  are  then 
estimated  by  finding  the  modes  that  have  near-zero  damping.  A  major  shortcoming  of 
this  approach  is  that  the  control  signal  needs  to  be  turned  off  while  the  input-output  data 
is  recorded,  otherwise  the  disturbance  modes  will  be  suppressed  by  the  control  signal, 
and  will  not  be  manifested  in  the  system  model.  Repeatedly  turning  off  the  control  signal 
will  likely  result  in  an  unacceptable  loss  of  performance.  Also,  for  time-invariant 
systems  the  full  system  model  does  not  have  to  be  recalculated  unless  the  system  has 
been  damaged  or  adjusted,  and  repeated  re-identification  is  computationally  inefficient. 

The  second  approach  to  estimating  the  disturbance  frequencies  requires 
fitting  an  autoregressive  (AR)  model  to  the  disturbance  effect  data.  Such  a  model  would 
have  the  form 


t](k)  =  Yxr\(k  - 1)  +  y2rj(& -2)  +  ...+yrrj(k-T)  (2-64) 


and  the  model  order  T  must  be  chosen  such  that 

qr>2f  (2-65) 
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where /is  the  number  of  disturbance  frequencies  present  in  The  AR  model  can 

then  be  analyzed  to  find  the  frequencies  of  the  modes  with  near-zero  damping  (the 
disturbance  frequencies).  This  can  be  done  in  a  similar  fashion  to  the  system 
identification  method  used  earlier  (conversion  to  modal  state-space  form,  etc.),  or  the 
roots  of  the  difference  equation  in  Eq.  (2-64)  can  be  used  to  find  the  z-plane  pole 
locations  (which  yield  the  frequencies  and  damping  ratios).  Since  the  disturbance  effect 
is  the  same  whether  or  not  the  control  system  is  operating,  there  is  no  need  to  turn  off  the 
controller  to  identify  the  disturbance  frequencies. 

By  adopting  a  feedforward  control  signal  of  the  form 

L 

«/<*)= Ek  cos(ft)(lAr)  +  bt  sin(<y(lAO]  (2-66) 

i=i 

(where  At  is  the  controller  sample  time)  it  is  possible  to  cancel  the  identified  disturbance 
frequencies  (Oi ,  i'  =  l,2,  ...,L.  To  solve  for  the  control  coefficients,  Eq.  (2-66)  is 
substituted  into  Eq.  (2-63)  to  obtain  a  set  of  equations  that  is  linear  in  a  and  b,  and  thus  a 
solution  can  be  obtained  recursively  in  real-time. 

Recognizing  that  periodic  re-identification  of  the  disturbance  frequencies 
could  result  in  abrupt  changes  in  the  0)l  values,  it  is  desirable  to  replace  the  term  a) ft  At 

in  (2-66)  with  9t(k)  which  is  updated  at  each  time  step  using 

0i(k)  =  9i(k-l)  +  A6i ,  and  A6>.  =<w,A t  (2-67) 
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Eq.  (2-63)  can  now  be  put  in  the  form 


r}(k)  = -<fj  {k)y  f 


(2-68) 


where 


ff  (k)  =  [Gx  (k), . . . ,  Gl  (k),  (A:)] ,  yf  = 


“l 

bi 


(2-69) 


and  the  qxm  matrices  G, ( k )  and  Ht(k) ,  i  =  1, 2, . . . ,  L ,  are  given  by 


Gt(k)  =  tfi  cos(0,(fc)-ZA0f),  H. (k)  =  '^j(5l  sin (di (k)- lAdt )  (2-70) 

1=1  i 


Equation  (2-68)  is  in  a  form  which  can  be  solved  with  Recursive  Least  Squares  (RLS) 
using  the  following  set  of  equations 


Wf  (k)  =Wf(k- 1)  +  Lf  (k)  [rj(k)  -  rj(k )]  (2-71) 
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where 


m=-ff(k)wf(k-i) 

Lf  (k)  =  Pf(k-  Y)<t>f  (k)  [</>;  (k)Pf  (k  - 1  )<t>f  (k) + XflqYq  ] 

/fX  \Pf(k-l)-Lf(k)ff(k)Pf(k-l)] 

Pf(k)~  Xf 

where  Xfis  the  “forgetting  factor”  which  determines  the  relative  data  weighting  [Ref. 
99]. 

As  soon  as  the  first  disturbance  frequency  estimate  is  available,  the 
recursive  solution  of  the  control  coefficients  is  initiated,  and  the  control  signal  is 
calculated  from  Eq.  (2-66),  using  the  0,  (£)  substitution  discussed  earlier.  The  initial  a, 

and  b(  values  are  set  to  zero,  and  the  initial  covariance  matrix,  Pf  (0)  =  Y^muimL  where  y 

is  large  compared  to  the  order  of  magnitude  of  the  parameters  to  be  estimated. 
Convergence  to  steady-state  values  will  occur  if  the  disturbance  frequencies  and 
amplitudes  are  constant  and  the  frequency  estimates  are  exact.  If  the  frequencies  and/or 
amplitudes  are  varying  with  time,  then  recursive  estimation  and  regular  disturbance 
frequency  estimates  allow  the  coefficients  to  track  the  correct  solution.  The  performance 
of  the  controller  depends  on  the  accuracy  of  the  frequency  estimates  provided.  If  the 
estimates  are  inaccurate,  or  the  true  disturbance  frequency  drifts  between  estimates,  the 
recursive  estimation  allows  the  coefficients  to  “cycle”  at  a  frequency  equal  to  the 
difference  between  the  true  and  estimated  frequencies. 


(2-72) 

(2-73) 

(2-74) 
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To  demonstrate  this,  assume  (Ot  is  the  true  disturbance  frequency,  and  (oe 
is  the  estimated  frequency,  and  define 

cda  =£0,  -o)e ,  or  ft),  =coe  +ft)A  (2-75) 

If  the  recursively  calculated  feedforward  control  signal  uf(k)  is  correctly  compensating 
for  the  disturbance,  as  will  be  demonstrated  in  Chapter  V,  then  it  must  be  composed  of 
sinusoidal  components  that  have  frequency  ft),  such  that 

uf{k)  =  ysin(ft),£)  +  <5cos(ft),£),  where  k  =  kAt  (2-76) 

However,  the  control  signal  is  calculated  from  estimates  of  the  disturbance  frequency, 
coe ,  and  so  we  also  have 

uf(k)  =  a(k)  sin(ft)e  k)  +  (3  ( k )  cos  (o)ek)  (2-77) 

where  a(k)  and  /3(k)  are  the  time-varying  feedforward  coefficients.  Equating  the  two 
expressions  for  uf(k) ,  we  obtain 

Y  sin(ft),fc)  +  8  cos  (o)tk)  =  a(k)sin(coek)  +  fi(k)cos((Oek)  (2-78) 
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Substituting  for  (0,  from  Eq.  (2-75)  we  have 


Y  sin((to€  +  (0A)k)  +  8  cos((ft)e  +  c oA)k )  =  a(k)sm((Qek)  +  f3(k)  cos  (coek) 


Using  trigonometric  identities  for  angle  summations,  and  rearranging  terms  yields 


y  [sin(cuefc)  cos  (coAk) + cos(a)ek)  sin(«A&)] + 

<5  [cos(ft)eA:)  cos  (p)Ak)  -  sin^fc)  sin(<wA£)]  =  a(k)  sin(£t)eA:)  +  f}(k)  cos  (coek) 


Grouping  terms  we  obtain 


[Y  cos(coAk)-S  sin(<oAfc)  -a(fc)]sin(ft)£A;)  + 

[Y  sin(o)Ak)  +  8  cos  (a>Ak)  -  P(k)]  cos  (coek)  =  0 


Setting  the  bracketed  terms  equal  to  zero,  and  using 


A  =  a//2+52 


0i 

02 


=  tan 


V 

f 

=  tan-1 

v 


r 

L 

8 


y 


we  arrive,  finally,  at  expressions  for  a(k )  and  (3  (k) ; 


(2-79) 


(2-80) 


(2-81) 


(2-82) 
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a(k )  =  Asin(coAfc  +  0,), 
P(k)  =  Asin(o)Ak  +02) 


(2-83) 


which  demonstrates  that  if  the  frequency  estimate  is  incorrect,  the  control  signal 
coefficients  will  vary  with  time,  and  will  cycle  sinusoidally  at  a  frequency  coA  equal  to 
the  difference  between  the  true  and  estimated  frequencies.  An  experimental 
demonstration  of  this  cycling  is  provided  in  Section  V.C.l.b). 

A  unique  feature  of  the  Clear  Box  algorithm  is  that  it  allows  selective 
control  of  each  identified  disturbance  frequency.  If  the  disturbance  frequency  happens  to 
be  located  at  a  frequency  which  is  uncontrollable  (or  weakly  controllable)  the  resulting 
control  signal  required  to  cancel  the  disturbance  would  have  a  large  magnitude,  which 
could  saturate  the  actuator(s).  To  prevent  this  from  happening,  logic  is  implemented  that 
prevents  any  attempt  to  control  these  frequencies.  To  accomplish  this,  the  disturbance 
frequency  in  question  is  simply  eliminated  from  the  list  of  disturbance  frequencies  sent  to 
the  DSP  controller.  In  general,  both  the  system  dynamics  and  the  disturbance  frequencies 
may  be  varying  with  time.  In  this  case,  disturbance  frequencies  can  be  selected  for 
control  based  on  the  amount  that  each  uncontrolled  disturbance  affects  the  response  of 
the  system,  and  also  on  the  magnitude  of  the  control  signal  required  to  control  the 
disturbance.  In  Eq.  (2-66),  the  Euclidean  norm  of  the  coefficients  associated  with  each 
sine/cosine  pair  represents  the  magnitude  of  the  control  signal  required  for  that 
disturbance  frequency. 
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To  determine  the  effect  each  disturbance,  if  uncontrolled,  would  have  on 


the  system  output,  Eq.  (2-60)  is  used  to  obtain 


yd  (*)  -axyd(k-\)-a2yd(k-2)-...-apyd(k-p)  =  r\(k)  (2-84) 

Assuming  the  system  is  linear,  it  is  known  that  the  steady  state  response  to  the 
disturbances  present  is  a  linear  combination  of  the  harmonic  components 

sin(ft),i:A0  +  dt  cos(ft),£A0]  (2-85) 

i=i 

Substituting  Eq.  (2-85)  into  Eq.  (2-84)  gives  an  expression  that  is  linear  in 
the  response  coefficients,  ci  and  di .  Batch  or  recursive  methods  can  be  used  to  solve  for 

these  coefficients,  and  the  Euclidean  norm  of  each  sine/cosine  pair  represents  the 
magnitude  of  the  system  response  for  each  disturbance  frequency.  Those  frequencies  that 
have  a  low  ratio  of  system  response  magnitude  to  control  signal  magnitude  can  thus  be 
de-selected  for  control  if  actuator  saturation  is  a  possibility. 


c)  Control  Formulation  Using  the  Disturbance  Effect  as  a  Basis  Set 
In  applications  where  the  disturbance  frequencies  are  known  to  vary,  an 
approach  that  does  not  rely  on  frequency  estimation  is  desirable.  Such  an  approach 
would  not  only  eliminate  the  processing  requirements  of  estimating  the  disturbance 


51 


frequencies,  but  would  also  improve  performance  since  the  frequency  estimates  would 
not  be  old  or  otherwise  inaccurate.  With  this  motivation,  a  new  Adaptive  Basis  Method 
is  presented  to  address  the  case  of  rapidly  varying  disturbances,  and  to  eliminate  the  need 
for  disturbance  frequency  estimates. 

Comparison  of  Eq.s  (2-41)  and  (2-60)  shows  that  all  of  the  frequencies 
eliminated  from  the  disturbance-corrupted  system  model  are  now  present  in  the 
disturbance  effect  term  rj(k) .  Consider  an  example  where  it  is  desired  to  control  a  single 
sinusoidal  disturbance  with  a  frequency  of  25  Hz  using  a  controller  operating  at  a 
sampling  rate  of  400  Hz.  This  means  there  will  be  16  data  samples  per  disturbance 
period,  and  7\(k)  will  contain  a  single  sinusoid  of  the  same  frequency  (25  Hz).  Shifting 
this  sinusoid  by  90  degrees,  or  4  samples,  results  in  an  orthogonal  signal  and  gives  a  basis 
set  similar  to  the  sine/cosine  functions,  and  a  controller  of  the  form 

uf  (k)  =  wMk) +W2V(k  ~  4)  (2-86) 

can  be  used  to  satisfy  the  controller  requirement  expressed  in  Eq.  (2-63).  The  y/1  and  y/2 
coefficients  can  be  solved  for  recursively  in  the  same  manner  as  a,  and  b{  in  Eq.  (2-69). 

Note  that  it  is  not  necessary  to  have  orthogonal  basis  functions  for  the 
controller  to  operate  successfully.  As  long  as  the  functions  are  not  linearly  dependent  (in 
the  example  this  would  occur  if  the  signal  were  shifted  by  a  multiple  of  8  samples),  then 
the  disturbance  can  be  completely  cancelled.  The  method  works  equally  well  when  there 
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are  /  disturbance  frequencies,  as  long  as  there  are  N  time-shifted  t](k)  basis  functions 
such  that 


iV>2/+l 


(2-87) 


For  the  general  case,  a  feedforward  control  signal  uf  ( k )  must  satisfy  Eq. 
(2-63),  which  is  repeated  here  for  convenience, 


Am/  (*-!)  +  P2 uf  (k  - 2)  + . . .  +  Ppuf  (k  -  p)  = 


The  assumed  form  of  the  control  signal  is 


uf  (k)  =  yq r)(k  -  A, ) + y/2r}(k  -  A2 )  + . . .  +  y/Nr](k  -  AN )  (2-88) 


where  A;  (i  =  1, ... ,  N)  are  the  number  of  samples  that  the  disturbance  effect  has  been 
shifted  to  generate  the  N  basis  functions.  Each  A,,  value  is  chosen  by  the  operator,  and 
the  guidelines  below  should  be  followed  when  selecting  them. 


A(.  >1  V  i 

A,.  *  A  J  V  i,  j 

A,-Ay.|^|Ay-A,|  V  i,j,k 
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(2-89) 


The  first  guideline  prevents  any  problems  with  causality  by  using  the  disturbance  effect 
that  is  delayed  by  at  least  one  time  sample.  The  second  ensures  that  two  functions  do  not 
have  the  same  time  shift  (such  a  pair  would  be  identical  functions).  The  third  introduces 
a  random  characteristic  to  the  time  shifting,  and  prevents  linear  dependence  of  the  basis 
functions  for  any  given  disturbance  frequency. 

Note  that  uf  (k)  is  rnx  1,  t]{k)  is  qxl,  and  thus  each  yr.  matrix  is  mxq. 

For  the  UQP  application  the  computational  requirements  of  the  algorithm  can  be  reduced 
by  noticing  that  the  disturbance  effect  signal  for  sensor  i  has  the  same  frequencies  as  the 
signal  in  sensor  j  ( V  i,  j ).  It  is  likely  that  the  disturbance  effect  signal  at  one  sensor  could 
have  amplitude  or  phase  differences,  but  each  strut’s  control  coefficients  can  be 
recursively  adjusted  to  account  for  unique  amplitude  and  phase  requirements.  Thus,  the 
rj(k )  time  history  for  any  strut  can  be  used  as  the  basis  function  for  controlling  all  of  the 
struts,  and  using  this  information  a  new  form  of  the  control  signal  is  chosen  as 

Uf  (k)  =  y/fl*  (k-  A, )  +  y/2ri*  (k  -  A2 ) + . . .  +  y/Nri*  (k  -  AN )  (2-90) 

where  T]*(k)  is  the  scalar  disturbance  effect  signal  at  the  strut  chosen  to  act  as  the  “basis 
strut”.  Now  each  y/i  parameter  (i  =  l, ... ,  N)  has  dimension  mx  1  instead  of  mxq .  The 

first  step  toward  solution  of  these  parameters  is  to  progressively  time  shift  Eq.  (2-90)  as 
follows 
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uf  (k-l)  =  yflri\k-A1-l)+W2rj\k-A2-l)  +  ...+  VNrj* (k-AN  - 1) 
uf{k-2)  =  yxr]\k-Al  -2)+ii/2/n*(k-A2-2)+...+y/N'n*(k-AN-2) 

uf  ( k-p)  =  W* (k-Al-p)+ y2r\  (k-A2- p)+ ...+ yNr\ * (k-AN  - p) 


Substitution  of  these  expressions  into  Eq.  (2-63)  results  in 


(k- Ai  - 1)  +  pxV2r)  (k- A2  - 1)  + . . .  +  PxWNn  (k-AN-l)  + 

P2W{rf (k- A,  -  2)  +  P2W2r\  (k - A2 - 2)  + . . .  +  j 82y/NV  (k-AN-2)  +  ...+  (2-92) 

PPViV  (k-Al-p)  +  ppy2r\ (k-A2-p)  +  ...  +  PpWNV  (k-AN-p)  =  -rj(k) 


Since  r)*(k)  is  a  scalar  sequence,  Eq.  (2-92)  can  be  rearranged  as 


M  (k-Ax- 1  )y/,  +  A  rf  (k-A2- 1  )W2  +  •  •  •  +  A  rf{k-AN-  l)yN  + 

M  (k-  A,  -  2)1//,  +  M  (k-  A2-  2  )i//2  +  ...  +  M(k-AN-  2)Vn  + . . .  +  (2-93) 

AX  (*  -  A,  -  p)vx  +  Ppr\  (k  -  A2  -  p)y2  + . . .  +  A PV  (k-AN-  p)yN  =-7]{k) 


and  rewritten  in  vector  form  as 


-T]{k)  =  {ky¥  (k) 


(2-94) 


where  we  define  the  following  vectors 
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(2-95) 


and 


<t>t  (*)  =  £  j8/(*  -  A,.  -  ;),  i  =  l,2,...,N 

j= i 


*(*)  = 


V'lW 

Vf2(*) 


(2-96) 


(2-97) 


Each  parameter  0(.(jfe)  is  a  gx/n  matrix,  making  &T(k)  a  qxNm  matrix.  The  vector 
W(k)  of  the  feedforward  control  parameters  has  dimension  Nmx  1 . 

Having  the  set  of  linear  equations  in  0,(fc)  in  the  form  of  Eq.  (2-94) 
facilitates  use  of  the  RLS  algorithm  described  below 


W(k)  =  V(k  - 1)  +  L(k)  [-7 i(k)  -  <f>T  (*)¥(*  - 1)] 
P(k)  =  P(k  - 1)  -  L(k)[< t>T  0 k)P(k  - 1)] 


(2-98) 


where 


L(k)  =  P(k  -  1)0(£)  [<Dr  ( k)P(k  -  m(k)  +  /?]'* 


(2-99) 
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A 

The  estimated  feedforward  control  coefficients  in  ¥(&)  are  used  in  the 
control  input  calculation  in  Eq.  (2-90).  The  computational  requirements  of  the  above 
algorithm  are  almost  identical  to  that  of  the  sine/cosine  controller  described  in  the 
previous  section,  except  there  is  no  need  to  compute  disturbance  frequency  estimates. 

In  the  case  where  there  are  uncontrollable  modes  known  to  be  present  in 
the  system,  or  if  actuator  control  ability  is  limited,  it  may  be  desirable  to  use  selective 
cancellation  of  disturbances  (as  with  the  sine/cosine  method).  This  can  be  implemented 
using  the  Adaptive  Basis  Method  through  selective  filtering  of  the  disturbance  effect 
signal  (demonstrated  in  Section  V.E.2). 

7.  Stability 

A  stability  analysis  conducted  in  Ref.  100  (for  a  SISO  system  controlling  a  single 
disturbance  frequency)  demonstrates  theoretically  that  the  Clear  Box  Sine/Cosine  Method 
has  a  phase  margin  of  ±90  degrees  at  the  frequency  of  the  disturbance,  assuming  a  slow 
adaptation  rate  (which  equates  to  a  forgetting  factor  close  to  one).  Equivalently,  the  error 
of  the  identified  system  dynamics  can  be  off  by  as  much  as  90  degrees  before  instability 
is  induced.  This  is  similar  to  the  results  of  stability  analyses  of  the  LMS-based 
algorithms  referenced  in  Section  B.4,  indicating  that  the  average  values  of  the  sensor 
error  and  the  disturbance-correlated  signal  (either  x(k)  or  t](k) )  must  at  least  be  of  the 

same  sign  in  order  for  the  error  to  be  reduced  [Ref.  101], 

The  Clear  Box  Adaptive  Basis  Method  is  very  similar  to  the  Sine/Cosine  Method 
in  the  case  of  a  single  disturbance  frequency  (assuming  a  low  level  of  noise  in  the 
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disturbance  effect  signal,  and  the  use  of  two  basis  functions,  or  N  =  2).  Thus  the 
stability  analysis  referenced  earlier  could  be  adapted  to  show  a  phase  margin  for  the 
Adaptive  Basis  Method  that  is  similar  to  the  other  two  controllers.  While  the  added 
nonlinear  effects  of  a  changing/adapting  set  of  controller  basis  functions  inhibits  analysis 
of  the  method  for  the  general  case,  experimental  assessment  of  the  controller’s  stability 
can  offer  some  indication  of  its  ability  to  perform  well  in  less  than  ideal  conditions.  This 
will  be  accomplished  in  Chapter  V. 


D.  ALTERNATE  APPROACHES 

Two  approaches  are  described  here  that  may  offer  alternative  ways  to  achieve 
good  performance  against  time- varying  frequencies  and/or  achieve  greater  computational 
efficiency.  These  approaches  are  recommended  for  implementation  in  future  work. 

The  Adaptive  Basis  Method  was  developed  to  allow  the  Clear  Box  Algorithm  to 
control  rapidly  varying  frequencies  with  little  or  no  loss  of  performance  (over  the  static 
frequency  case).  This  same  goal  can  be  achieved  if  the  Sine/Cosine  Method’s  frequency 
estimates  are  significantly  more  accurate.  To  achieve  this,  the  most  recent  disturbance 
frequency  estimates  can  be  fit  to  a  polynomial  curve.  This  curve  can  then  be  extrapolated 
forward  in  time  to  the  time  steps  prior  to  the  next  frequency  update.  This  is  shown  in 
Figure  II-7  using  a  simple  third  order  polynomial  fit  to  eight  data  points  of  past 
disturbance  frequency  estimates.  In  this  hypothetical  situation,  the  last  estimate  was 
given  at  time  step  £  =  8000,  and  the  next  update  is  not  expected  until  time  step 
k  =  9000 .  The  current  approach  of  the  Sine  Cosine  Method  (as  implemented  on  the 
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UQP)  holds  the  last  estimate  until  the  next  update,  in  which  case  the  estimate  becomes 
increasingly  inaccurate  as  time  progresses.  The  proposed  approach  allows  extrapolation 
by  evaluating  the  polynomial  for  the  current  time  step  at  each  time  step  until  the  next 
update  is  available.  At  that  time  the  polynomial  is  re-fit  to  the  available  data,  and  the 
process  repeated  until  the  next  update.  The  resulting  improvement  in  frequency 
estimation  should  allow  better  performance  in  the  case  of  rapidly  varying  frequencies. 
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Figure  II-7:  Improving  the  Sine/Cosine  Method's  Frequency  Estimates  Using  a 

Polynomial  Curve  Fit 


The  second  proposed  approach  involves  forming  a  hybrid  controller  that 
uses  the  disturbance  effect  signal  rj(k)  from  the  Clear  Box  Algorithm  to  act  as  the 
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reference  signal  x(k)  for  the  Multiple  Error  LMS  Algorithm.  The  block  diagram  for 
such  a  controller  is  shown  in  Figure  II-8. 


Figure  II-8:  Hybrid  Controller  Block  Diagram 

The  advantages  of  this  approach  are  that  the  identification  provided  by  the 
Clear  Box  component  allows  control  of  time-varying  systems  since  the  Clear  Box  system 
model  can  be  translated  into  the  FIR  filter  form  needed  by  the  LMS  algorithm.  Control 
of  unanticipated  disturbances  and  harmonics  is  possible  since  this  information  is 
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available  in  the  disturbance  effect  signal,  without  the  need  for  the  separate  sensor  to 
provide  the  disturbance  correlated  signal.  At  the  same  time  the  processing  requirements 
are  significantly  less  than  that  of  the  Clear  Box  Algorithm,  although  slightly  more  than 
the  Multiple  Error  LMS  Algorithm  since  the  disturbance  effect  must  be  calculated  at  each 
time  step.  Also,  the  selective  control  capability  is  available  through  filtering  of  the  r\{k) 
signal. 

A  disadvantage  of  the  hybrid  method  is  that  the  order  of  the  FIR  model 
required  to  control  lightly  damped  systems  may  still  be  impractically  large,  and  thus  the 
required  increase  in  the  order  of  the  model  would  negate  some  of  the  processing  gains 
associated  with  this  approach.  Also,  although  the  selective  disturbance  control  option  is 
available,  the  decision  regarding  which  disturbances  to  control  is  slightly  hindered  by  the 
fact  that  the  magnitude  of  the  control  signal  needed  to  control  each  disturbance  is 
unknown  (since  sine  &  cosine  coefficients  are  not  used).  The  decision  would  have  to  be 
made  based  on  noted  transmission  zeros  in  the  system  model,  and  the  relative 
significance  of  each  disturbance  frequency  to  the  total  system  output  (similar  to  the 
approach  used  with  the  Adaptive  Basis  Method). 


E.  SUMMARY 

The  wide  acceptance  of  the  FXLMS  (and  extension  to  the  MIMO  Multiple  Error 
LMS)  algorithm  for  active  control  of  sound  and  vibration  is  based  on  its  ease  of 
implementation  and  adequate  performance  in  many  applications.  However,  the  Clear 
Box  control  formulation  approaches  the  problem  from  a  system  identification 
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perspective,  allowing  greater  insight  into  the  physical  processes,  and  selectivity  in 
controlling  disturbances. 

The  next  chapter  discusses  the  experimental  setup,  and  Chapter  IV  discusses  the 
results  of  the  system  identification  experiments.  Both  the  Multiple  Error  LMS  Algorithm 
and  Clear  Box  Algorithm  (including  both  methods  discussed  above)  are  implemented  on 
the  UQP  in  experiments  conducted  in  Chapter  V,  and  the  observed  advantages  and 
disadvantages  are  discussed  in  Chapter  VI. 
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III.  EXPERIMENT  SETUP 


A.  UQP 

The  “Ultra  Quiet  Platform”,  or  UQP,  on  which  the  experiments  were  performed 
was  built  by  CSA  Engineering  of  Palo  Alto,  CA,  and  is  configured  similarly  to  a  six 
degree  of  freedom  “cubic”  Stewart  Platform.  In  such  a  system,  the  struts  are  arranged  as 
if  they  were  on  the  edges  of  a  cube,  thus  providing  for  three  orthogonal  pairs  of  actuators. 
The  advantage  of  such  an  arrangement  is  that  control  in  six  degrees  of  freedom  is 
possible  using  all  linear  actuators,  and  actuator  coupling  is  minimized,  [ref.  102] 


Figure  III-l:  UQP  and  Satellite  Bus  Mockup 
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The  platform  is  mounted  on  a  spacecraft  bus  mockup,  to  which  is  mounted  an 
Aura  bass  shaker  which  serves  as  the  disturbance  source.  The  entire  experiment  sits  on 
sixteen  rubber  feet  attached  to  a  3800  lb.  Newport  RS4000  isolation  table.  The  table  is 
mounted  on  four  Newport  1-2000  series  Laminar  Flow  Isolator  pneumatic  pedestals 
which  help  to  further  isolate  the  experiment  from  floor  vibrations. 

1.  Smart  Struts 

The  six  struts  that  support  the  top  plate  each  consist  of  an  actuator,  sensor,  and  a 
passive  isolation  stage  (shown  in  Figure  IH-2).  The  struts  are  mounted  on  each  end  using 
flexible  mounts  to  minimize  the  transmission  of  bending  moments. 


Flexible 

Mount 


Figure  III-2:  Smart  Strut  Configuration 
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a)  Actuators 

The  piezoceramic  stack  actuators  (PZTs)  in  each  strut  convert  control 
signal  voltages  to  physical  movement  of  the  strut.  The  maximum  displacement  of  the 
actuators  is  50  |xm,  which  is  enough  for  vibration  isolation  applications,  but  not  for 
platform  pointing/steering.  Although  piezoceramics  are  considered  “hard”  actuators,  the 
intended  application  is  narrowband  disturbance  rejection  and  hard  or  soft  actuators  work 
equally  well. 


b)  Sensors 

The  Geospace  GS-11D  geophone  sensors  consist  of  wire  coils  supported 
by  soft  springs  under  the  influence  of  a  magnetic  field,  and  the  sensors  provide  a  signal 
proportional  to  velocity.  The  GS-11D  model  has  a  natural  frequency  of  14  Hz  (double 
pole  with  damping  factor  £=0.8),  above  which  the  sensitivity  is  fairly  constant.  Below  14 
Hz  the  response  decays  rapidly. 

The  geophone  sensors  were  selected  for  this  experiment  because  of  their 
low  cost  and  ruggedness.  An  aerospace  quality  accelerometer  might  be  a  more  logical 
choice  for  an  actual  spacecraft  application,  and  would  extend  the  useable  control 
bandwidth  to  lower  frequencies. 
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c)  Passive  Isolation 

A  degree  of  passive  isolation  is  achieved  through  use  of  a  flexure  with 
damping  material.  This  passive  isolation  stage  is  in  series  with  the  active  stage,  and  the 
resulting  six  platform  suspension  modes  have  frequencies  between  25-80  Hz.  An 
advantage  of  this  series  configuration  is  that  if  a  flexible  structure/payload  is  mounted  on 
the  top  of  the  platform,  the  passive  stage  tends  to  minimize  any  control/structure 
interaction.  Additionally,  if  a  strut  experiences  on-orbit  failure  there  exists  some  degree 
of  isolation  from  the  spacecraft  bus. 

2.  Disturbance  Source 

The  source  of  disturbances  for  the  disturbance  rejection  experiments  is  an  Aura 
bass  shaker  (model  AST-1B-4,  25  W,  4  ohm).  The  shaker  is  mounted  to  the  underside  of 
the  spacecraft  bus’  top  plate  (Figure  III-3),  and  is  driven  by  sinusoidal  signals  generated 
by  the  digital  signal  processor  (DSP).  The  disturbance  generator  code  can  be  modified  to 
output  a  variety  of  disturbance  profiles,  from  single  static  frequencies  to  multiple 
disturbance  frequencies  that  have  time-varying  amplitudes  and  frequencies. 


B.  SUPPORT  ELECTRONICS 

1.  Hardware  Interface 

The  UQP  experiment  requires  power  amplification  for  the  actuators  and  signal 
conditioning  for  the  geophone  sensors.  These  are  provided  via  a  PCB  Piezotronics 
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790A06  6-channel  power  amplifier  (±  200  V,  ±  100  mA),  and  a  CSA  Engineering  Active 
Vibration  Control  System  (AVCS)  signal  conditioning  unit  supplied  with  the  UQP.  The 
disturbance  generator  is  powered  by  a  Kepco  BOP  20-10M  amplifier.  Figure  HI-3  shows 
the  configuration  of  the  experiment. 


Figure  III-3:  Experiment  Overview 
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2.  Digital  Signal  Processor 

The  control  function  is  performed  by  the  combination  of  a  dSPACE  DSP  system 
and  a  host  PC  (see  Figure  HI-4).  The  dSPACE  system  includes  an  “alpha  combo” 
multiprocessor  system  consisting  of  a  Texas  Instruments  C40  50  MHz  processor  with 
512KB  of  memory,  and  a  Digital  Equipment  Corporation  Alpha  500  MHz  processor  with 
2MB  of  memory.  The  C40  performs  all  of  the  input/output  functions  such  as  interfacing 
with  the  analog  to  digital  converter  (ADC)  and  digital  to  analog  converter  (DAC)  boards, 
and  data  transfer  to  and  from  the  host  PC. 


Figure  III-4:  Control  Processing 
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The  ADC  is  a  DS2003  32  channel  unit  with  the  resolution  set  at  12  bits.  The 
DAC  is  a  DS2103  (also  32  channels),  with  the  resolution  fixed  at  14  bits.  On  both  units 
the  input/output  voltage  range  can  be  selected  as  either  ±5  volts,  or  ±10  volts.  To  prevent 
aliasing,  the  ADC  samples  data  at  a  rate  of  10  kHz  and  then  digital  anti-aliasing  filters 
(3rd  order  Chebyshev)  are  employed  on  the  C40  board  with  a  comer  frequency  of  200  Hz. 
Analog  filtering  before  the  ADC  is  unnecessary  due  to  the  low  system  response  above  5 
kHz  (see  further  discussion  in  Chapter  IV).  This  arrangement  provides  5x  oversampling 
of  the  highest  frequencies  passed  (unattenuated)  by  the  filters.  Low-pass  filters  are  again 
employed  on  the  output  to  smooth  the  “stair-step”  quality  of  the  control  signal.  This 
prevents  any  excessive  excitation  of  the  actuators  at  the  1  kHz  sample  rate  of  the  control 
signal  calculation. 

3.  Host  Personal  Computer  (PC) 

The  host  PC  (A  Dell  Optiplex  GX1,  450MHz)  serves  several  functions.  Coding 
for  the  control  algorithms  is  performed  in  the  Matlab/Simulink  environment  using  C- 
coded  “S-Functions”  to  perform  the  more  specialized  tasks.  dSPACE  software  on  the 
host  PC  allows  automatic  DSP  code  generation  and  downloading  when  working  in  the 
Matlab  environment.  While  the  controller  is  running  on  the  DSP,  the  host  PC  performs 
supervisory  functions  and  also  interfaces  with  the  user. 

Before  the  start  of  each  experiment  the  user  is  given  the  option  to  perform  a 
complete  system  identification  (see  Chapter  IV).  Once  the  identification  is  complete  the 
host  PC  downloads  the  updated  system  model  to  the  DSP.  While  the  controller  is 
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running  on  the  DSP  the  PC  also  performs  periodic  estimates  of  the  disturbance 
frequencies  (when  using  the  Clear  Box  Sine/Cosine  Method),  and  downloads  these 
updated  estimates.  Experiment  data  is  also  captured  by  the  PC  using  the  dSPACE 
MTrace  utility.  Multiple  captures  can  be  performed  simultaneously  allowing  data  to  be 
collected  for  disturbance  frequency  updates  at  the  same  time  as  different  variables  are 
captured  for  post-experiment  analysis. 


C.  SOFTWARE 

Many  different  software  tools  were  used  during  the  process  of  coding, 
monitoring,  and  analysis  (summarized  in  Table  DI-1).  The  dSPACE  software  was 
designed  to  work  with  Matlab  and  Simulink,  and  allows  rapid  transition  from  a  “block 
diagram”  representation  of  the  control  algorithm  to  real-time  code  running  on  the  DSP. 
Except  for  compilation  of  the  C-coded  S-Functions,  all  code  generation  and  downloading 
is  handled  by  the  dSPACE  RTI  software  (working  in  combination  with  the  Matlab  Real 
Time  Workshop). 

The  MT.TR  and  MTrace  utilities  from  dSPACE  allows  the  host  PC’s  experiment 
supervisor  (Matlab  code)  to  acquire,  process,  store,  and  download  data  while  the  DSP 
was  running.  This  allows  a  high  degree  of  automation  to  be  built  into  the  experiment. 

The  Simulink  block  diagram  is  used  to  divide  functions  between  the  C40  and 
Alpha  processors  by  separating  the  tasks  with  “Inter-Processor  Communication”  (IPC) 
blocks.  More  details  are  provided  in  Chapters  IV  and  V. 
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Software  Tools 

Version 

Function 

Matlab 

5.2.1.1420 

Real-Time  Workshop 

2.2.1 

Real-time  code  generation 

Simulink 

2.2.1 

Algorithm  development 

dSPACERTI  1003 

3.2 

Real-time  interface  to  Simulink 

dSPACE  RTI-MP 

3.2 

Real-time  interface  for  multiprocessor 
systems 

MTrace 

3.1 

Data  acquisition  from  DSP 

MLIB 

3.1 

Data  downloading  to  DSP 

Compilers 

AXP-GCC  GNU 

2.7.2 

C  compiler  for  DEC  Alpha 

TMS-320 

4.70 

Compiler  for  TI C40 

Microsoft  Visual  C++ 

5.0 

Compiler  for  S-Functions 

Table  III-l :  Software  Versions  and  Functions 
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IV.  SYSTEM  IDENTIFICATION  EXPERIMENTS 


A.  EARLY  IDENTIFICATION  WORK 

System  identification  experiments  performed  in  [Ref.  103]  on  the  UQP  at  the 
Naval  Postgraduate  School  resulted  in  six  SISO  models  from  the  input  of  each  actuator’s 
amplifier  to  the  output  of  the  corresponding  strut’s  sensor.  An  example  of  the  frequency 
response  obtained  from  this  work  (see  Figure  IV- 1)  shows  that  there  is  only  one  lightly 
damped  mode  which  dominates  the  response  at  1.4  kHz,  and  a  smaller  one  just  above  1 
kHz.  The  remainder  of  the  response  appears  well  damped.  The  response  lacks  some 
fidelity  below  10  Hz  due  to  1)  sensor  noise  ,  2)  limited  data  record  length,  and  3)  limited 
model  order.  The  data  record  length  is  limited  since  a  relatively  high  sample  rate  (5  kHz) 
was  needed  to  capture  the  1.4  kHz  mode,  and  the  available  memory  was  finite. 


Figure  IV-1:  Frequency  Response,  Strut  #1  Early  Model 
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B.  SAMPLE  RATE  SELECTION 


The  disturbance  frequencies  of  interest  for  the  UQP  application  are  approximately 
20-300  Hz,  and  the  system  model  must  be  accurate  in  that  frequency  range.  To  improve 
the  fidelity  of  the  model  at  low  frequencies  the  sample  rate  is  reduced,  allowing  longer 
time-duration  records.  Another  contributing  factor  is  that  the  real  time  control 
calculations  are  relatively  complex,  given  that  a  6-input,  6-output  MIMO  controller  is 
used.  These  factors  led  to  a  decision  to  reduce  the  sampling  rate  from  5  kHz  (used  in  the 
previous  identification  work)  to  1  kHz. 


C.  ANTI-ALIASING  FILTERS 

At  sampling  frequencies  below  ~3  kHz  aliasing  of  the  1.4  kHz  mode  occurs,  and 
thus  it  is  clear  that  anti-aliasing  filters  are  needed.  As  discussed  in  Chapter  1H,  and 
shown  in  Figure  HI-4,  the  use  of  a  10  kHz  sample  rate  for  the  A/D  conversion  allows 
accurate  capturing  of  all  frequencies  below  5  kHz.  This  allows  digital  anti-aliasing  filters 
to  be  used  before  the  sample  rate  is  reduced  to  1  kHz.  The  main  advantage  of  using 
digital  filters  is  the  ability  to  rapidly  reconfigure  the  filter  to  any  desired  type,  comer 
frequency,  and  order.  The  disadvantage  is  that  it  requires  processing,  and  for  a  spacecraft 
application  where  processing  power  is  scarce  it  may  be  advantageous  to  use  analog  filters 
(weight  and  reliability  must  also  be  considered). 

For  this  project  we  have  chosen  3rd  order  Chebyshev  filters  with  1  dB  of  passband 
ripple  and  a  200  Hz  comer  frequency.  This  configuration  gives  sufficient  attenuation  of 
the  system  response  above  the  Nyquist  frequency  of  500  Hz. 
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D.  SYSTEM  ID  DATA  COLLECTION 


To  facilitate  the  acquisition  of  system  identification  data,  a  program  was  coded 
for  the  DSP  (controlled  by  the  Host  PC)  that  first  turns  on  a  white  noise  source  (input  to 
all  six  struts),  and  then  records  input-output  data  as  long  as  possible  according  to  the 
available  memory.  Recording  twelve  channels  of  data  at  a  rate  of  1  kHz,  the  longest 
allowable  data  record  is  1 1  seconds. 

Figure  IV-2  shows  the  Simulink  block  diagram  which  represents  the  source  for 
the  DSP  code.  The  white  noise  source  is  simply  a  six-channel  random  number  generator 
with  normal  distribution  and  zero  mean.  Also  present  are  a  disturbance  generator  which 
can  generate  up  to  five  sinusoidal  disturbances.  These  disturbances  can  be  constant  or 
time-varying  in  frequency  and  amplitude,  depending  upon  user  responses  to  prompts 
from  the  Host  PC.  An  impulse  generator  function  allows  the  unit  pulse  response  to  be 
obtained  for  each  strut,  enabling  comparison  with  the  identified  model’s  pulse  response. 

All  of  these  functions  are  located  in  “enabled  subsystem”  Simulink  blocks  which 
allow  them  to  be  turned  on  and  off  based  on  commands  sent  from  the  Host  PC.  The  Host 
PC  code  also  starts  and  stops  all  data  recording,  and  returns  the  data  to  the  workspace  for 
subsequent  system  identification  calculations. 
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Figure  IV-2:  System  Identification  Simulink  Diagram 

Note  in  Figure  IV-2  that  a  DC  offset  is  applied  to  the  actuator  input  signal.  This 
is  needed  to  keep  the  drive  signal  in  the  middle  of  the  actuators’  operating  range  of  0-100 
Volts,  thus  an  offset  of  50  V  is  applied.  The  “IPC”  blocks  are  for  InterProcessor 
Communication  between  the  Texas  Instruments  C40  board  (the  elements  below  the  IPC 
blocks)  and  the  Digital  Equipment  Corp.  Alpha  board  (the  elements  above  the  IPC 
blocks).  The  C40  is  operating  at  10  kHz,  and  the  Alpha  is  operating  at  1  kHz,  which 
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requires  a  sampling  rate  transition  when  going  from  one  to  the  other.  The  zero-order  hold 
and  unit  delay  blocks  provide  this  transition.  The  system  model  identified  from  the 
input-output  data  takes  into  account  the  added  delay  due  to  the  zero-order  hold  and  unit 
delay  transition  blocks. 

The  “sensor  signal  processing”  is  the  anti-alias  filtering  discussed  earlier.  Also  of 
note  is  the  “control  signal  processing”  that  occurs  prior  to  D/A  conversion.  In  the  case 
where  there  is  a  sinusoidal  input  to  the  actuator,  the  effect  of  the  sampling  rate  transition 
is  a  sinusoid  with  stair  steps  occurring  once  every  sampling  period  at  the  lower  rate. 
Without  filtering,  this  stair  step  effect  excites  the  actuators  at  the  slow  sampling 
frequency  (1  kHz)  which  degrades  performance.  The  solution  is  to  employ  digital  low 
pass  filtering  in  a  manner  similar  to  that  used  for  the  anti-aliasing  filters.  After  some 
experimentation  it  was  determined  that  good  “smoothing”  was  obtained  using  a  4th  order 
Chebyshev  lowpass  filter  at  400  Hz.  The  amount  of  delay  caused  by  the  filtering  is 
frequency  dependent,  but  it  is  on  the  order  of  one  half  of  the  lower  sampling  period 
(1/2*0.001  sec),  as  can  be  seen  in  Figure  IV-3.  The  disturbance  signal  is  also  filtered 
before  it  goes  to  the  shaker  (2nd  order  Chebyshev  lowpass  at  300  Hz)  to  prevent  the 
unwanted  1  kHz  disturbance  from  exciting  the  platform. 
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Figure  IV-3:  Effect  of  Low  Pass  Filtering  on  Control  Signals 


E.  OKID  REFERENCE  MODEL 

To  obtain  an  initial  MIMO  model  of  the  UQP  system  dynamics  the 
Observer/Kalman  Filter  Identification  (OKID)  software  is  used  to  generate  a  state  space 
model.  This  model  also  serves  to  verify  the  identification  results  obtained  from  the  Clear 
Box  algorithm  in  the  next  section.  There  is  extensive  literature  available  on  OKID  [Ref.s 
104,105,106].  Suffice  it  to  say  that  the  technique  serves  to  obtain  an  accurate  estimate  of 
the  system’s  state  space  model  from  a  time  history  of  input-output  data.  For  this  reason  it 
is  referred  to  as  a  “time-domain”  approach. 

The  desired  model  order  is  determined  by  the  selection  of  p,  the  equivalent  order 
of  the  model  for  each  input-output  pair.  For  the  reference  model,  p  has  been  selected  as 
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40  which  (for  q  =  6 )  results  in  a  MEMO  state  space  model  with  an  A  matrix  of  dimension 
240x240 .  Shown  below  in  Figure  IV-4  is  the  frequency  response  at  the  sensor  outputs 
of  all  six  struts  for  an  input  to  the  strut  #1  actuator  (the  frequency  response  is  very  similar 
for  inputs  to  the  other  struts).  The  uppermost  magnitude  plot  is  for  the  output  at  the  strut 
#1  sensor.  This  is  to  be  expected  since  it  is  most  directly  coupled  to  the  strut  #1  actuator. 
The  effect  of  the  anti-aliasing  filters  at  200  Hz  can  be  seen  by  the  sharp  reduction  of  the 
response  above  this  frequency. 

The  remaining  five  plots  in  Figure  IV-4  show  the  coupling  that  exists  between  the 
strut  #1  actuator  and  the  sensors  on  the  other  five  struts.  In  general  the  coupling  is 
strongest  with  the  neighboring  struts  on  either  side,  and  in  the  frequency  range  of  30-150 
Hz.  The  realization  that  this  coupling  is  strongest  within  the  intended  control  bandwidth 
led  to  a  decision  to  implement  a  full  MIMO  controller  instead  of  six  SISO  controllers. 

A  notable  system  characteristic  is  a  steadily  decreasing  response  below  the 
Geophone  sensor’s  natural  frequency  of  14  Hz.  This  puts  a  lower  limit  on  the  useful 
control  bandwidth  of  the  UQP.  Also  notable  are  several  damped  modes  from  30-100  Hz, 
which  are  suspension  modes  due  to  the  passive  isolation  stage  on  each  strut.  These 
modes  may  be  altered  in  a  zero-g  environment,  and  in  a  spacecraft  application  the  system 
identification  would  need  to  be  re-accomplished  upon  reaching  orbit. 
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Bode  Diagram  for  Input  to  Strut  #1 


Freq  [Hz] 


Figure  IV-4:  OKID  Reference  Model  Frequency  Response 


F.  CHECKING  THE  ACCURACY  OF  THE  REFERENCE  MODEL 

Two  methods  are  used  to  verify  that  the  OKID  model  obtained  above  is  an 
accurate  representation  of  the  UQP  system’s  dynamics.  First,  the  true  pulse  response  of 
the  system  is  obtained  and  compared  against  that  of  the  model.  Second,  a  new  set  of 
input-output  data  is  obtained,  and  the  actual  system  output  is  compared  to  that  of  the 
model  (using  the  same  input  data). 
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1.  Pulse  Response 

Using  the  pulse  response  capabilities  of  the  Simulink  code  shown  in  Figure  IV-2, 
the  true  “UQP  pulse  response”  is  obtained  by  inputting  an  impulse  to  one  strut  at  a  time. 
Using  the  OKID  state  space  reference  model  obtained  above,  the  corresponding  “model 
pulse  response”  is  generated.  These  responses  (36  input-output  pairs)  match  closely,  as 
can  be  seen  by  the  examples  shown  in  Figure  IV-5  and  Figure  IV-6  below. 


Impulse  Response:  Input  Strut  #3,  Output  Strut  #3 


Figure  IV-5:  Impulse  Response  of  Model  vs.  Actual  (#1) 


81 


Impulse  Response:  Input  Strut  #2,  Output  Strut  #3 


Figure  IV-6:  Impulse  Response  of  Model  vs.  Actual  (#2) 

2.  Response  to  Random  Input  Data 

Another  test  of  the  reference  model’s  accuracy  is  accomplished  through  use  of  a 
set  of  “verification  data”,  which  is  different  from  the  data  used  to  generate  the  identified 
model.  A  second  set  of  UQP  response  data  is  obtained  using  random  inputs,  and  the 
input  data  is  then  applied  to  the  OKID  reference  model.  If  the  model  is  accurate,  the 
resulting  simulated  output  should  match  that  of  the  UQP  hardware.  The  samples  shown 
below  in  Figure  IV-7  show  that  the  outputs  of  the  model  match  those  of  the  UQP  very 
well.  After  the  first  0.02  seconds  (the  system’s  settling  time),  the  two  plots  are  almost 
indistinguishable.  At  this  point  it  can  be  stated  that  the  OKID  model  is  of  sufficient 
accuracy  to  serve  as  a  reference  against  which  other  identification  results  can  be 
compared. 
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Simulated  vs.  Actual  Outputs:  Strut  #1 


Figure  IV-7:  Actual  vs.  Simulated  Response  to  Random  Inputs 

G.  DISTURBANCE  SOURCE  FREQUENCY  RESPONSE 

Using  the  same  technique  as  in  Section  E  above,  the  response  of  the  six  strut 
sensors  to  inputs  to  the  Aura  bass  shaker  (the  disturbance  source)  is  obtained.  None  of 
the  control  algorithms  implemented  require  a  model  of  this  “primary  plant”,  but  it  is 
useful  for  determining  which  frequencies  cause  the  largest  system  response. 

A  set  of  input-output  data  is  obtained  for  the  shaker-to-sensor  system,  and  the 
resulting  OKID  state  space  model  is  used  to  obtain  the  frequency  response  in  Figure  IV-8 
below. 
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Input  to  Shaker,  Output  at  Sensors 


Freq  [Hz] 


Figure  IV-8:  Shaker  Frequency  Response 

From  this  response  it  is  clear  that  the  sensors  are  most  sensitive  to  disturbance 
frequencies  at  50  and  103  Hz.  In  general,  struts  1,  4,  5,  and  6  are  more  sensitive  to 
disturbances  due  to  their  proximity  to  the  shaker.  Struts  2  and  3  are  on  the  opposite  side 
of  the  platform  from  the  shaker,  which  explains  their  lower  sensitivity.  There  is  a 
significant  dip  in  the  response  of  strut  #3  to  a  90  Hz  disturbance. 
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H.  CLEAR  BOX  MODEL 


As  outlined  in  Section  II.C.5  on  page  32,  the  Clear  Box  algorithm  takes  the 
approach  of  first  identifying  the  system  model  (even  in  the  presence  of  periodic 
disturbances  and  sensor  noise).  The  result  is  a  “disturbance  free”  model  which  can  be 
expressed  in  any  chosen  form  (state  space,  ARX,  etc.). 

As  a  test  of  the  algorithm’s  ability  to  correctly  identify  the  UQP  system  dynamics 
model  in  the  presence  of  disturbances,  a  new  set  of  input-output  data  is  obtained  while 
the  shaker  (disturbance  source)  is  driven  by  a  signal  consisting  of  two  sinusoids  at  70  and 
103  Hz.  These  frequencies  were  chosen  to  show  that  the  algorithm  will  detect  with  equal 
accuracy  disturbance  frequencies  to  which  the  system  is  least  and  most  sensitive  (see 
Figure  IV-8).  The  data  is  then  used  to  construct  a  disturbance-corrupted  model,  the 
frequency  response  of  which  is  shown  in  Figure  IV-9.  Note  the  effects  of  the  two 
disturbance  modes  at  70  and  103  Hz. 
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Figure  IV-9:  Clear  Box  System  ID,  Disturbance-Corrupted 

The  disturbance-corrupted  model  is  then  converted  to  modal  form  so  that  each 
mode  can  be  analyzed.  The  top  portion  of  Figure  IV-10  shows  the  frequencies  of  the 
identified  modes  and  their  associated  damping  ratios.  The  disturbance  modes  are 
identified  by  the  fact  that  they  are  below  the  damping  threshold,  which  is  set  by  the  user. 
The  bottom  portion  of  Figure  IV-10  shows  the  relative  contribution  of  each  mode  to  the 
total  system  pulse  response.  The  modes  with  a  greater  “modal  pulse  response  norm”  are 
the  true  system  modes,  and  the  remainder  are  the  noise  and  disturbance  modes. 
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Disturbance-Corrupted  System  Mode  Analysis,  p  =  40 


0  50  100  150  200  250  300  350  400  450  500 


Frequency  (Hz) 

Figure  IV-10:  Clear  Box  System  ID,  Analysis  of  Modes 

As  discussed  in  Section  n.C.5.a)  (starts  on  page  33),  there  are  three  types  of 
modes  present;  true  system  modes,  disturbance  modes,  and  noise  modes.  The  true  system 
modes  are  determined  by  their  large  contribution  to  the  system’s  pulse  response.  In  this 
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case  the  anti-aliasing  filters  used  at  200  Hz  tend  to  dominate  the  response  due  to  the 
highly  damped  nature  of  the  UQP  in  this  low  frequency  region. 

After  identifying  the  disturbance  modes,  they  are  removed  from  the  system 
model.  The  resulting  model  has  the  frequency  response  shown  in  Figure  IV-11,  which 
matches  very  closely  with  that  of  the  OKID  reference  model  shown  earlier  in  Figure 
IV-4.  The  noise  modes  can  be  removed  if  desired,  although  retaining  them  has  the  effect 
of  filtering  the  noise  associated  with  the  system  response  [Ref.  107]. 


Input  to  Strut  #1 


Figure  IV-11:  Clear  Box  System  ID,  Disturbance-Free 
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I. 


CONVERSION  TO  ARX  MODEL 


Now  that  the  disturbance-free  state  space  model  has  been  determined  (through 
elimination  of  the  disturbance  modes)  the  methods  of  Section  n.C.5.c)  on  page  38  are 
used  to  convert  the  model  to  ARX  form  for  implementation  on  the  digital  signal 
processor.  The  model  has  the  form 


y(k)  =  aly(k-l)+a2y(k-2)  +  ...+apy(k-p)  + 

- 1)  +  &«(*-  2)  + ...  +  ppu(k -p)+ rj(k) 


(4-1) 


where,  again,  the  a  and  ft  coefficients  are  the  disturbance-free  system  model,  and  r\{k) 
is  the  disturbance  effect.  This  form  of  the  model  is  used  for  the  Clear  Box  algorithm, 
which  is  implemented  in  the  next  Chapter.  The  disturbance  effect,  which  is  calculated  by 
rearranging  Eq.  (4-1)  above  such  that 


t]  (k)  =  y  (k)  -  ax  y  (k  - 1)  -  a2  y  (k  -  2) - . . .  -  ap  y  (k  -  p) 
-p1u(k-l)-P2u(k-2)-...-Ppu(k-p) 


(4-2) 


plays  a  key  role  in  both  the  “Sine/Cosine  Method”  and  the  “Adaptive  Basis  Method”  of 
the  Clear  Box  algorithm  (discussed  in  Section  n.C.6  starting  on  page  42).  It  contains  all 
of  the  information  needed  to  cancel  any  disturbance  frequency  that  is  not  retained  in  the 
system  model  coefficients. 
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J.  CONVERSION  TO  FIR  FILTER  MODEL 


Recall  from  Section  II.B.3  (page  20)  that  the  Multiple  Error  LMS  algorithm 
requires  a  Model  in  the  Form  of  a  Finite  Impulse  Response  (FIR)  filter.  Such  a  model  is 
obtained  directly  from  the  disturbance-free  state  space  model  obtained  earlier  in  this 
Chapter,  using  either  the  OKID  reference  model  or  the  Clear  Box  model.  In  practice  the 
method  used  to  determine  the  disturbance-free  model  depends  on  whether  or  not  the 
disturbances  can  be  turned  off  while  data  is  taken.  If  the  disturbances  are  always  present, 
then  the  Clear  Box  model  is  the  only  alternative  which  achieves  accurate  results. 

To  obtain  an  FIR  model  using  the  disturbance-free  state  space  model,  we  simply 
calculate  the  system  Markov  parameters  (pulse  response  coefficients)  using  the 
A,  B,  C,  D  matrices.  Recall  that  if  there  are  M  actuators  and  L  sensors,  then 


y(k)  -  CmQu(k)  +  Cmxu(k  1  )+'“+ClM(J_1)U(k  (7  1)) 
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In  the  case  of  the  Multiple  Error  LMS  algorithm,  the  FIR  model 
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(4-6) 
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is  used  to  filter  the  disturbance-correlated  reference  signal  x(k)  to  give  the  “filtered-*” 
signal  r(k)  where 


rtm(k)  =  ^clmjx(k-j)  (4-7) 

J=o 

This  signal,  along  with  the  sensor  error  e(k)  is  used  in  the  LMS  algorithm  to  adapt  the 
feedforward  controller’s  filter  coefficients  w(k)  (see  Eq.  (2-22)). 


K.  SUMMARY 

In  this  chapter  sets  of  input-output  data  were  used  to  build  a  model  of  the  UQP 
system  dynamics.  In  this  case  the  “system”  includes  all  subsystems  such  as  the  actuators, 
struts,  sensors,  filters,  rate  transition  delays,  and  any  dynamics  associated  with  D/A  and 
A/D  conversion.  To  obtain  a  reference  model,  band-limited  white  noise  inputs  were 
used,  and  outputs  obtained  in  the  absence  of  any  user-generated,  or  other  disturbances 
(there  are  always  some  disturbances  associated  with  room  and  sensor  noise).  The 
resulting  OKID  model  was  found  to  have  a  pulse  response  that  matched  that  of  the  actual 
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system,  and  a  set  of  verification  data  confirmed  that  the  model  and  the  actual  system  react 
very  similarly  to  random  inputs. 

The  Clear  Box  algorithm’s  system  identification  routine  was  used,  in  the  presence 
of  sinusoidal  disturbances,  to  generate  a  disturbance-corrupted  model  which  was 
converted  to  a  disturbance-free  model  using  disturbance  mode  elimination.  The  model 
was  then  converted  to  ARX  form  for  implementation  on  the  DSP. 

Finally,  it  was  shown  how  the  model  can  be  transformed  into  a  Finite  Impulse 
Response  filter,  which  is  the  form  of  the  model  needed  for  implementation  of  the 
Multiple  Error  LMS  algorithm.  The  next  chapter  will  demonstrate  how  these  models  are 
used  in  disturbance  rejection  experiments  on  the  UQP. 
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V.  DISTURBANCE  REJECTION  EXPERIMENTS 


A.  INTRODUCTION 

The  experiments  performed  in  this  chapter  were  designed  to  reveal  the  strengths 
and  weaknesses  of  the  three  controllers  investigated  in  this  research.  The  Multiple  Error 
LMS  algorithm,  the  recently  developed  Clear  Box  algorithm’s  “Sine/Cosine  Method”, 
and  the  Clear  Box  “Adaptive  Basis  Method”  (developed  during  the  course  of  this 
research)  are  all  demonstrated  in  a  variety  of  disturbance  environments. 

The  initial  experiments  are  with  static  disturbances  that  do  not  change  their 
amplitude  or  frequency  (a  variety  of  single  and  multiple  disturbance  cases  are  explored). 
The  next  set  of  experiments  show  how  the  controllers  perform  with  slowly  and  rapidly 
varying  disturbances.  Finally,  consideration  is  given  to  the  case  of  an  uncontrollable 
system  mode,  and  what  happens  when  the  disturbance  frequency  coincides  with  that 
mode.  Discussion  of  the  results  of  these  experiments  is  presented  in  Chapter  VI. 

B.  IMPLEMENTATION 

1.  Multiple  Error  LMS 

The  Multiple  Error  LMS  algorithm  was  implemented  on  the  UQP  using  the 
Matlab/Simulink  environment  as  discussed  in  Chapter  m.  The  Simulink  block  diagram 
is  shown  in  Figure  V-l,  which  contains  many  of  the  same  building  blocks  as  the  code 
used  for  the  system  identification  experiments  performed  in  Chapter  IV.  In  this  case  the 
controller  requires  a  disturbance-correlated  signal  to  function  properly.  A  simple  unit 
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delay  is  used  to  model  a  sensor  transfer  function,  and  the  resulting  signal  is  referred  to  as 
x(k)  or  “the  reference  signal”.  The  “Controller”  subsystem  in  the  Simulink  diagram 
consists  of  the  “C”,  “W”,  and  “LMS  Algorithm”  blocks  in  Figure  II-5,  and  uses  C  code  to 
implement  the  Multiple  Error  LMS  Algorithm  in  accordance  with  Eq.  (2-22). 


Figure  V-l:  Multiple  Error  LMS  Controller  Simulink  Diagram 

2.  Clear  Box 

The  Clear  Box  Algorithm  (either  method)  is  implemented  in  much  the  same  way 
as  the  Multiple  Error  LMS  Algorithm.  The  most  significant  difference  is  that  the  Clear 
Box  controllers  do  not  require  a  sensor  to  provide  a  disturbance  correlated  reference 
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signal  (Figure  V-2).  The  algorithm  provides  its  own  disturbance  correlated  signal 
through  calculation  of  the  disturbance  effect,  as  discussed  in  Section  n.C.5.c).  White 

noise  excitation  is  added  to  the  system  input  when  taking  data  for  system  identification. 

\ 

Although  system  ID  can  be  accomplished  as  often  as  necessary,  it  is  generally  only 
accomplished  at  the  beginning  of  an  experiment.  The  Sine/Cosine  Method  of  Clear  Box 
requires  additional  processing  to  identify  the  disturbance  frequencies,  and  this  function  is 
performed  by  the  host  PC  using  data  downloaded  from  the  DSP. 


Enable 

Disturbance 


Figure  V-2:  UQP  Clear  Box  Controller  Simulink  Diagram 
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C.  REJECTING  STATIC  DISTURBANCES 

The  first  set  of  experiments  demonstrates  the  relative  ability  of  each  controller  to 

reject  a  constant  sinusoidal  disturbance  at  a  single,  unchanging  frequency.  This  case 
allows  a  methodical  investigation  into  the  effects  of  altering  any  of  the  available 
parameters  (adaptation  rate,  recursive  forgetting  factor,  model  order,  etc.)  The  second  set 
of  experiments  shows  behavior  in  the  presence  of  multiple  disturbance  frequencies. 

1.  Single  Static  Disturbance  Frequency 
a)  Multiple  Error  LMS 

The  first  experiment  attempts  to  cancel  a  120  Hz  disturbance  which  is 
imparted  on  the  system  by  means  of  the  Aura  bass  shaker  mounted  to  the  bottom  of  the 
satellite  bus  mockup,  to  which  the  UQP  is  mounted.  The  amplitude  of  the  disturbance  is 
chosen  such  that  there  is  a  significant  output  at  the  strut  sensors,  without  going  beyond 
the  linear  range  of  the  shaker.  To  do  so  would  introduce  harmonics  of  the  fundamental 
frequency,  which  is  a  case  that  will  be  explored  later.  The  results  of  the  first  experiment 
can  be  seen  in  Figure  V-3,  which  shows  all  six  strut  sensor  outputs  during  the  course  of 
the  experiment,  with  the  controller  being  initiated  at  approximately  0.5  seconds.  The 
result  is  a  40  dB  drop  in  the  overall  RMS  level  of  the  sensor  outputs. 

A  more  revealing  way  to  view  the  result  of  the  control  action  is  to  look  at 
the  power  spectrum  of  one  of  the  sensors  (strut  #1  was  selected)  before  and  after  the 
controller  is  activated  as  shown  in  Figure  V-4.  Clearly  the  energy  at  120  Hz  has  been 
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removed  from  the  sensor  signal.  There  is,  however,  some  (minor)  negative  impact  on  the 
frequencies  immediately  surrounding  the  disturbance  frequency. 

The  control  signals  for  the  six  struts,  shown  in  Figure  V-5,  are  determined 
by  filtering  the  reference  signal.  The  coefficients  for  this  control  filter  W  converge  via 
gradient  descent  to  the  “optimal”  solution  at  a  rate  that  is  determined  by  the  value  of  the 
adaptation  rate  n  and  (as  will  be  seen  later)  by  the  mean  square  level  of  the  filtered 
reference  signal  r(k) .  In  this  experiment  the  adaptation  rate  was  selected  as  0. 1 .  Figure 
V-6  shows  the  coefficients  associated  with  strut  #1.  In  this  case  a  tenth  order  filter  is 
used,  but  for  a  single,  zero-mean  disturbance  frequency  only  two  are  required.  Note  that 
each  of  the  six  sensors  has  a  small  offset  bias  that  is  removed  for  data  display  purposes. 


Figure  V-3:  Multiple  Error  LMS  Results  for  a  120  Hz  Disturbance 
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Figure  V-4:  Power  Spectrum  Comparison,  120  Hz  Disturbance 
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Figure  V-6:  Coefficient  Convergence,  |Li=  0.08, 120  Hz  Disturbance 

The  selection  of  }X  has  a  great  impact  on  the  performance  of  the 
controller.  To  demonstrate  this,  two  more  experiments  are  run  using  different  adaptation 
rates.  A  lower  rate  of  0.01  (Figure  V-7)  shows  that  slower  convergence  results,  and  the 
coefficient  values  are  also  less  “noisy”.  Increasing  the  rate  to  0.13  results  in  instability 
(Figure  V-8),  so  care  must  be  taken  to  chose  a  conservative  (low)  adaptation  rate. 

Also  important  to  the  selection  of  the  adaptation  rate  is  a  knowledge  of  the 
bounds  on  the  disturbance  frequencies  to  be  controlled.  A  given  adaptation  rate  ju 
results  in  greatly  different  performance  for  high  and  low  frequency  disturbances.  The 
reason  for  this  is  that  the  mean  square  level  r 2  of  the  filtered  reference  signal  r(k) 
increases  with  increasing  frequency  since  the  magnitude  of  the  plant  model  generally 
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increases  with  frequency  (Figure  IV- 11).  Recall  that  the  upper  bound  for  jU  for  stable 

operation  is  inversely  related  to  r2  (Eq.  (2-23)),  and  thus  the  effective  adaptation  rate  is 
affected  by  the  disturbance  frequency.  As  a  demonstration,  the  original  adaptation  rate  of 
0.08  is  used  on  two  disturbances,  one  at  40  Hz,  and  another  at  150  Hz.  The  resulting 
coefficient  conversion  is  seen  in  Figure  V-9  and  Figure  V-10.  The  controller  converges 
very  slowly  for  the  40  Hz  disturbance,  and  exhibits  instability  for  any  frequency  at  or 
above  150  Hz.  In  general,  the  best  performance  for  a  given  disturbance  frequency  is 
obtained  by  “tuning”  the  adaptation  rate  in  a  manner  inversely  proportional  to  7 2 . 


Figure  V-7:  Coefficient  Convergence,  mu=0.01, 120  Hz  Disturbance 
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Figure  V-10:  Coefficient  Convergence,  mu=0.08, 150  Hz  Disturbance  (Unstable) 

Returning  to  the  case  of  the  120  Hz  disturbance,  it  is  possible  to 
characterize  the  steady  state  performance  by  looking  at  the  RMS  level  of  each  strut’s 
sensor,  and  comparing  it  to  that  of  the  sensor’s  background  noise  level.  After  each 
experiment  is  completed,  a  brief  pause  allows  the  transients  to  die  out,  and  a  reading  of 
the  “background”  sensor  noise  levels  is  taken.  These  noise  levels  change  depending  on 
the  vibration  and  acoustic  environment  in  the  room,  but  the  RMS  level  is  generally  from 
0.012  -  0.016  Volts.  Figure  V-ll  shows  that  the  overall  sensor  output  RMS  level  is 
reduced  to  a  level  just  above  that  of  the  sensor  noise,  down  40  dB  from  the  pre-control 
level  of  1.61  Volts. 
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Figure  V-ll:  Steady  State  and  Sensor  Noise  Levels,  120  Hz  Disturbance 

To  fully  characterize  the  performance  of  the  Multiple  Error  LMS 
algorithm  for  a  single  static  frequency  disturbance,  experiments  are  run  over  a  range  of 
frequencies  using  three  different  values  for  I,  the  control  filter  order.  In  each  experiment, 
the  adaptation  rate  is  tuned  for  best  performance.  The  results  are  shown  in  Figure  V-12, 
which  shows  both  the  dB  reduction  in  the  RMS  level  for  each  frequency,  and  the  relative 
“position”  of  the  steady  state  levels  in  comparison  to  the  sensor  noise  level  (positive 
levels  are  above  the  noise  level).  Note  that  the  amount  of  dB  drop  is  related  to  the  initial 
level  of  the  disturbance,  so  if  the  same  voltage  signal  is  sent  to  the  shaker  for  each 
frequency,  then  the  response  will  vary  according  to  the  frequency  response  of  the  shaker 
(Figure  IV-8  on  page  84).  Clearly  the  dB  drop  in  Figure  V-12  is  related  to  this  shaker 
frequency  response.  Thus  the  more  accurate  measure  of  the  performance  of  the  controller 
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is  to  compare  the  steady  state  noise  level  with  the  sensor  noise  level.  Note  that  the  filter 
order  has  little  effect  on  the  controller  performance  for  a  single,  static  disturbance 
frequency. 


Frequency  [Hz] 


Figure  V-12:  Multiple  Error  LMS  Performance  vs.  Frequency 


Finally,  to  test  the  stability  of  the  Multiple  Error  LMS  Algorithm,  an 
impulsive  disturbance  is  imparted  to  the  experiment  while  the  controller  is  operating. 
Return  of  the  control  coefficients  to  pre-control  levels  would  give  an  indication  of 
stability.  Figure  V-13  and  Figure  V-14  show  the  resulting  sensor  outputs  and  control 
coefficients,  respectively.  They  indicate  that  the  Multiple  Error  LMS  Algorithm  is  stable 
for  this  type  of  disturbance. 
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Figure  V-13:  Multiple  Error  LMS,  Sensor  Outputs  -  Impulsive  Disturbance 


Figure  V-14:  Multiple  Error  LMS,  Control  Coefficients  -  Impulsive  Disturbance 
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Also  tested  was  the  impact  of  plant  modification  on  the  controller’s 
performance  by  adding  a  significant  amount  of  mass  to  the  top  of  the  platform.  This  had 
very  little  impact  on  the  performance  of  the  Multiple  Error  LMS  controller,  and  the  filter 
weights  were  able  to  adapt  to  new  optimal  values.  Similar  experiments  to  those 
performed  above  will  be  performed  on  the  UQP  in  the  next  two  sections  using  the  Clear 
Box  Algorithm  (both  methods),  and  comparisons  between  controllers  will  be  made  in 
Chapter  VI. 

b )  Clear  Box,  Sine/Cosine  Method 

The  implementation  of  the  Clear  Box  algorithm  was  presented  briefly  in 
Section  B.2.  Recall  from  Section  HC.6.b)  starting  on  page  44  that  the  Sine/Cosine 
Method  forms  a  control  signal  by  recursively  estimating  the  coefficients  of  sine/cosine 
pairs  (one  pair  for  each  disturbance  frequency).  The  steady  state  values  of  the 
coefficients  should  result  in  a  minimization  of  the  error  signals  at  the  sensor  outputs. 

The  first  experiment  using  the  Sine/Cosine  Method  uses  the  same  120  Hz 
constant  disturbance  frequency  as  that  used  earlier  with  the  Multiple  Error  LMS 
algorithm.  The  recursive  estimation  process  uses  a  “forgetting  factor”  X  described  in  Eq. 
(2-74),  and  an  initial  covariance  associated  with  the  coefficients.  Also  a  “threshold  level” 
for  the  damping  ratio  must  be  selected,  below  which  the  identified  frequency  component 
of  the  disturbance  effect  signal  is  considered  a  disturbance. 

The  results  of  the  first  experiment  (controlling  a  static  120  Hz  disturbance) 
can  be  seen  in  Figure  V-15,  where  the  control  action  is  initiated  at  t  =  0.5  seconds. 
Similar  results  to  the  Multiple  Error  LMS  algorithm  are  achieved,  including  a  rapid 
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convergence  and  reduction  of  the  disturbance  by,  in  this  case,  40  dB.  In  the  frequency 
domain  (Figure  V-16)  it  is  seen  that  a  similar  (perhaps  more  narrow)  notch  is  formed  at 
the  disturbance  frequency  as  was  seen  in  Figure  V-4.  However,  there  is  less  of  an 
adverse  impact  of  the  controller  on  frequencies  immediately  surrounding  the  disturbance. 
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Figure  V-15:  Clear  Box  Sine/Cosine  Sensor  Outputs,  120  Hz  Disturbance 
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Frequency  [Hz] 

Figure  V-16:  Sine/Cosine  Spectral  Comparison,  120  Hz  Disturbance 

The  control  signals  used  to  drive  the  six  actuators  are  shown  together  in 
Figure  V-17.  Each  strut’s  control  signal  is  generated  by  a  sine/cosine  pair  for  each 
disturbance  frequency,  so  in  this  case  there  are  twelve  coefficients  (two  per  strut)  that 
must  be  recursively  estimated.  For  the  single,  120  Hz  disturbance  the  time  histories  of 
the  two  recursively  calculated  coefficients  for  strut  #1  are  shown  in  Figure  V-18.  In  this 
case  A  (the  forgetting  factor)  was  set  to  0.98.  A  value  closer  to  1  gives  a  “smoother” 
history  that  responds  less  quickly  to  changes  in  the  calculated  disturbance  effect,  and  a 
smaller  value  gives  a  more  “noisy”  history.  For  a  static  disturbance,  the  choice  of 
forgetting  factor  does  not  have  a  great  deal  of  impact  on  the  performance,  unless  the 
disturbance  frequency  estimate  is  incorrect  (demonstrated  later  in  this  section). 
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In  a  manner  similar  to  that  used  with  the  Multiple  Error  LMS  experiments, 
the  RMS  level  of  the  steady  state  sensor  outputs  is  calculated  for  each  strut,  and  overall. 
As  is  seen  in  Figure  V-19  the  results  are  similar  to  those  obtained  using  Multiple  Error 
LMS.  The  overall  RMS  level  is  only  slightly  higher  than  the  level  of  the  noise. 

Similar  experiments  were  performed  over  a  range  of  frequencies,  and  the 
results  are  plotted  in  Figure  V-20.  The  performance  is  very  similar  to  that  of  the  Multiple 
Error  LMS  controller,  controlling  the  disturbance  close  to  the  noise  level  in  all 
frequencies  tested.  Again,  the  dB  drop  is  seen  to  correspond  roughly  to  the  shaker 
frequency  response.  In  general,  there  are  no  parameters  that  need  to  be  “tuned”  to  achieve 
good  performance,  in  contrast  to  the  Multiple  Error  LMS  algorithm  where  the  adaptation 
rate  must  be  selected  based  on  the  frequency  of  the  disturbance  (see  previous  section). 
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Figure  V-19:  Steady  State  and  Sensor  Noise  Levels,  120  Hz  Disturbance 
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Figure  V-20:  Clear  Box  Sine/Cosine  Method,  Performance  vs.  Frequency 

The  stability  of  the  Sine/Cosine  Method  is  tested  by  imparting 
disturbances  to  the  platform.  The  controller  is  very  robust  with  respect  to  disturbances, 
and  even  heavy  shaking  of  the  platform  does  not  cause  instability.  A  sample  of  the  six 
sensor  outputs  and  the  strut  #1  control  coefficients  are  shown  in  Figure  V-21  and  Figure 
V-22,  respectively. 

Also  tested  was  the  impact  of  plant  modification  on  the  controller 
performance.  Addition  of  significant  mass  to  the  top  of  the  platform  had  very  little 
impact  on  the  performance  of  the  Sine/Cosine  Method  controller,  and  the  coefficients 
were  able  to  adapt  to  new  optimal  values. 


Ill 


Strut  Sensor  Outputs 

81 - 1 - 1 - ! - 1 - 1 - 


6 


4 


time  [sec] 


Figure  V-21:  Sine/Cosine  Method,  Sensor  Outputs  -  Impulsive  Disturbances 


Sample  of  Control  Signal  Coefficients 


Figure  V-22:  Sine/Cosine  Method,  Control  Coefficients  -  Impulsive  Disturbances 
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The  role  that  the  forgetting  factor  plays  in  the  performance  of  the 
Sine/Cosine  controller  depends  on  the  accuracy  of  the  disturbance  frequency  estimate.  To 
demonstrate  this,  a  static  disturbance  frequency  of  150  Hz  is  used,  and  the  estimate  of  the 
frequency  is  deliberately  set  at  various  values  (identified  by  percent  error).  The  result  is 
seen  in  Figure  V-23,  which  shows  that  more  accurate  disturbance  frequency  estimates 
help  improve  performance. 

The  forgetting  factor  becomes  an  important  tool  for  improving 
performance  of  the  Sine/Cosine  controller  when  the  frequency  estimate  is  incorrect. 
“Forgetting”  more  of  the  past  data  allows  the  control  signal  coefficients  to  “cycle”  and 
minimize  the  sensor  error  (see  discussion  in  Section  n.C.6.b).  A  demonstration  of  the 
effect  is  accomplished  with  a  static  disturbance  by  keeping  the  error  of  the  frequency 
estimate  constant,  and  then  using  a  variety  of  forgetting  factors  to  see  how  performance  is 
effected.  Figure  V-24  uses  data  for  four  different  error  levels  to  show  that  improved 
performance  is  obtained  by  using  a  smaller  forgetting  factor  (recall  that  a  smaller 
forgetting  factor  provides  more  “data  forgetting”).  Use  of  a  forgetting  factor  lower  than 
0.65  has  little  additional  impact,  however.  The  coefficient  “cycling”,  discussed  earlier  in 
Section  HC.4,  is  shown  in  Figure  V-25  demonstrating  that  the  coefficients  oscillate  at  a 
frequency  equal  to  the  difference  between  the  actual  and  the  estimated  frequency  (in  this 
case  the  frequency  estimate  was  off  by  1%  of  100  Hz,  so  the  cycling  was  at  1  Hz). 
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Figure  V-24:  Effect  of  Forgetting  Factor  on  Sine/Cosine  Method  Performance 
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Figure  V-25:  Demonstration  of  Coefficient  Cycling 

Note  that  during  the  course  of  the  single  static  disturbance  frequency 
experiments  using  the  Sine/Cosine  controller,  a  typical  error  in  the  estimate  of  the 
disturbance  frequency  was  0.001%  and  was  always  less  than  0.02%.  Thus  for  static 
frequencies  the  issue  is  not  critical.  However,  if  the  disturbance  frequency  is  changing 
rapidly,  then  the  forgetting  factor  plays  an  important  role  in  achieving  good  performance, 
as  will  be  demonstrated  in  Section  D.l.b)  starting  on  page  134. 

c)  Clear  Box,  Adaptive  Basis  Method 

The  Adaptive  Basis  Method  controller  is  implemented  in  a  fashion 
identical  to  that  of  the  Sine/Cosine  Method.  The  difference  between  the  two  is  that  the 
Adaptive  Basis  Method’s  recursive  algorithm  estimates  control  signal  coefficients  for  a 
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basis  that  consists  of  the  time-shifted  disturbance  effect  signal,  instead  of  pairs  of  sine 
and  cosine  functions.  Estimation  of  the  disturbance  frequency(ies)  is  not  required  since 
this  information  is  included  in  the  disturbance  effect  signal. 

The  first  experiment  with  the  Adaptive  Basis  controller  is  identical  to  that 
conducted  in  the  previous  sections.  A  constant  120  Hz  disturbance  is  imparted  to  the 
UQP  via  the  bass  shaker.  Recursive  estimation  of  the  control  coefficients  results  in 
minimization  of  the  error  signal  at  the  sensor  outputs,  as  shown  by  the  time  history  of  the 
six  sensor  outputs  in  Figure  V-26.  Inspection  of  the  spectral  content  of  the  strut  #1  error 
signal  before  and  after  the  controller  is  activated  (Figure  V-27)  reveals  that  the  120  Hz 
signal  is  completely  eliminated.  A  small  amount  of  amplification  is  present  in  frequency 
bands  centered  around  AC  power  frequency  multiples  (i.e.,  60  Hz  x  k,  k  =  1, 2, . . . ). 
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Figure  V-26:  Clear  Box  Adaptive  Basis  Controller  Results  for  a  120  Hz  Disturbance 
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Figure  V-27:  Power  Spectrum  Comparison,  120  Hz  Disturbance 


The  sinusoidal  control  signals  for  the  six  actuators  are  shown  in  Figure 
V-28.  The  control  signals  represent  a  linear  combination  of  the  r]  shifted  time  histories. 
As  discussed  in  H.C.6.c),  a  single  strut’s  disturbance  effect  time  history  can  serve  as  the 
basis  for  all  six  actuators’  control  signals.  The  coefficients  for  each  strut  adapt  as  needed 
to  account  for  unique  gain  and  phase  requirements.  In  this  experiment  six  time-shifted  tj 
signals  are  employed  (N  =  6),  and  the  convergence  of  their  associated  coefficients  is 
shown  in  Figure  V-29.  If  the  sensor  signals  and  the  calculated  rj  signal  are  noise-free 
and  zero-mean,  two  time-shifted  signals  are  sufficient  to  completely  cancel  the 
disturbance.  In  this  case  performance  is  improved  by  using  redundant  time  shifted 
signals  (demonstrated  later). 
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In  a  manner  similar  to  the  experiments  with  the  other  controllers,  the 
steady  state  RMS  level  of  the  sensor  signals  can  be  compared  with  the  sensor  noise  level, 
which  can  vary  as  the  room  environment  changes.  Thus,  the  sensor  noise  level  may  be  at 
different  levels  for  different  experiments.  Figure  V-30  reveals  that  for  this  experiment, 
the  steady  state  sensor  RMS  level  was  actually  below  the  sensor  noise  level  (discussed  in 
Chapter  VI). 

One  parameter  that  the  user  can  choose  is  the  number  of  time  shifted 
disturbance  effect  signals  N  to  use  in  forming  the  control  signal.  Since  the  coefficients 
must  be  estimated  for  each  time  shifted  signal,  increasing  the  number  of  shifts  also 
increases  the  computational  burden  on  the  processor.  Using  a  model  order  p  of  15  the 
maximum  number  of  time  shifts  that  can  be  handled  by  the  alpha  processor  operating  at  1 
kHz  is  6.  Performing  experiments  at  various  disturbance  frequencies  using 
N  =  2, 4,  and  6  allows  the  overall  performance  of  the  Adaptive  Basis  controller  to  be 
characterized,  and  is  shown  in  Figure  V-31.  Clearly,  using  more  time  shifts  improves  the 
performance  and  extends  the  frequency  range  over  which  disturbances  can  be  effectively 
controlled. 
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Figure  V-31:  Adaptive  Basis  Method  Performance  vs.  Frequency 
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To  assess  the  stability  of  the  controller  while  in  operation,  an  impulsive 
disturbance  is  applied  to  the  top  of  the  UQP  platform  while  controlling  a  120  Hz 
disturbance.  Figure  V-32  and  Figure  V-33  show  the  sensor  output  and  control  coefficient 
time  histories,  respectively,  and  reveal  that  pre-impulse  conditions  are  re-established  after 
a  short  adjustment  period.  Using  a  forgetting  factor  less  than  one  reduces  the  time 
required  to  recover,  but  may  adversely  effect  steady-state  performance.  Note  that  very 
large  impulsive  disturbances  were  observed  to  cause  instability  of  the  controller.  The 
effects  of  plant  modification  (addition  of  mass  to  the  platform)  were  also  tested  on  the 
Adaptive  Basis  Method  controller,  with  very  little  impact  on  performance. 
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Figure  V-32:  Adaptive  Basis  Method,  Sensor  Outputs  -  Impulsive  Disturbance 
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Figure  V-33:  Adaptive  Basis  Method,  Control  Coefficients  -  Impulsive  Disturbance 

Recall  that  for  the  Sine/Cosine  Controller  the  recursive  algorithm’s 
forgetting  factor  greatly  effects  performance  when  the  disturbance  frequency  estimate  is 
in  error,  but  not  when  the  estimate  is  exact  (Figure  V-24  on  page  1 14).  In  the  case  of  the 
Adaptive  Basis  controller  there  are  no  frequency  estimates  required,  but  the  recursive 
algorithm  can  still  use  a  forgetting  factor,  allowing  recent  data  to  be  more  heavily 
weighted  than  past  data.  Using  the  same  dB  drop  and  RMS  level  comparisons  used 
earlier,  Figure  V-34  shows  the  performance  of  the  Adaptive  Basis  Method  against  a  static 
150  Hz  disturbance  using  various  forgetting  factors.  The  results  indicate  that  using  all 
past  data  improves  performance  in  the  case  of  a  disturbance  with  constant  frequency  (to 
be  discussed  in  Chapter  VI). 
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Figure  V-34:  Effect  of  Forgetting  Factor,  150  Hz  Disturbance 

Another  factor  that  can  affect  the  Adaptive  Basis  controller’s  performance 
is  the  accuracy  of  the  identified  system  model,  since  the  model  is  used  in  the  calculation 
of  the  disturbance  effect  and  the  control  signal  coefficients.  The  accuracy  of  the  model  is 
improved  by  using  a  larger  model  order  p.  The  effect  on  controller  performance  can  be 
seen  from  the  data  in  Figure  V-35  which  was  compiled  from  two  sets  of  experiments 
against  disturbances  at  90  Hz  and  140  Hz.  The  disturbance  amplitudes  were  adjusted  so 
that  the  magnitude  of  the  sensor  response  (without  control)  was  the  same  for  both 
disturbances.  In  both  sets  of  data  the  model  order  is  increased  from  one  experiment  to 
the  next.  The  general  trend  is  toward  improved  performance  with  larger  p,  especially  at 
the  lower  frequency  (90  Hz).  In  general,  performance  improves  with  increasing  p  and  N. 
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Figure  V-35:  Adaptive  Basis  Method,  Performance  vs.  Model  Order 
2.  Multiple  Static  Disturbance  Frequencies 

There  are  two  cases  of  “multiple  static  disturbances”  that  are  explored  in  this 
section.  The  first  demonstrates  how  each  controller  can  handle  the  case  of  two  distinct 
disturbance  frequencies.  The  second  is  a  case  where  there  is  a  single  disturbance 
frequency  that  also  generates  harmonic  disturbances. 

a)  Multiple  Error  LMS 

Two  disturbance  frequencies,  at  95  and  160  Hz,  are  present  for  the  first 
experiment.  For  the  Multiple  Error  LMS  algorithm  the  adaptation  rate  must  be  chosen 
based  on  the  highest  frequency  (so  that  the  system  will  remain  stable).  Figure  V-36  and 
Figure  V-37  below  show  the  output  history  and  spectral  content  of  the  sensor  signals, 
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respectively.  The  conservative  choice  of  adaptation  rate  results  in  a  slightly  lengthened 
period  of  convergence,  but  it  is  still  less  than  0.5  seconds.  Once  again  the  disturbances 
are  reduced  to  a  level  comparable  to  the  sensor  noise  level.  The  spectral  comparisons 
show  complete  elimination  of  the  disturbance  energy  at  the  two  disturbance  frequencies 
of  95  Hz  and  160  Hz. 
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Figure  V-36:  Multiple  Error  LMS  Sensor  Outputs,  2  Static  Disturbances 
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Figure  V-37:  Multiple  Error  LMS  Spectral  Comparison,  2  Static  Disturbances 

The  second  experiment  with  multiple  disturbances  is  a  case  where  the 
fundamental  disturbance  frequency  is  50  Hz,  and  two  harmonics  are  present  at  100  Hz 
and  150  Hz.  In  such  a  case  the  50  Hz  signal  is  sent  to  the  shaker  and  also  acts  as  the 
reference  signal  x(k) ,  after  passing  through  the  unit  delay  which  represents  the  sensor 
transfer  function  (Figure  V-l  on  page  94).  The  dynamics  of  the  shaker-to-sensor  system 
at  50  Hz  are  such  that  harmonic  disturbances  are  also  generated.  These  harmonic 
frequencies  are  not  present  in  the  x(k)  signal.  Figure  V-38  below  shows  the  before/after 
comparison  of  the  spectral  content  of  the  sensor  signal  for  strut  #1.  Clearly  the 
fundamental  frequency  is  controlled,  but  the  harmonics  at  100  Hz  and  150  Hz  are  not. 
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Figure  V-38:  Spectral  Comparison,  Multiple  Error  LMS  Controlling  Harmonics 
b)  Clear  Box,  Sine/Cosine  Method 

The  same  two  cases  used  above  are  tested  using  the  Sine/Cosine  Method 
of  the  Clear  Box  algorithm.  The  first  experiment  is  with  two  distinct  frequencies  at  95 
Hz  and  160  Hz.  The  sensor  outputs  are  shown  in  Figure  V-39,  and  the  before/after 
spectral  content  of  the  strut  #1  sensor  is  shown  in  Figure  V-40.  Use  of  a  forgetting  factor 
less  than  one  helps  improve  performance,  and  in  this  case  the  value  was  set  to  0.99.  The 
fact  that  the  control  signal  is  formed  from  sine  and  cosine  functions  results  in  no  impacts 
at  frequencies  other  than  the  disturbance  frequencies. 
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Figure  V-39:  Sine/Cosine  Method  Sensor  Outputs,  2  Static  Disturbances 


Figure  V-40:  Sine/Cosine  Method  Spectral  Comparison,  2  Static  Disturbances 
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For  the  case  of  harmonic  disturbances,  the  Clear  Box  Algorithm  (either 
method)  has  an  advantage  over  the  Multiple  Error  LMS  algorithm  since  it  uses  the 
calculated  disturbance  effect  signal.  In  calculating  the  disturbance  effect  signal,  the 
controller  essentially  estimates  the  outputs  that  are  not  caused  by  the  known  actuator 
inputs.  As  such,  the  fundamental  disturbance  and  its  harmonics  are  present  in  the 
disturbance  effect  signal  The  Sine/Cosine  Method  estimates  the  disturbance 

frequencies  based  on  the  r\(k)  signal,  and  controls  each  one  as  if  it  were  a  separate 
disturbance.  The  before/after  spectral  comparison  (again  using  the  strut  #1  sensor)  in 
Figure  V-41  clearly  indicates  complete  control  of  all  three  disturbances. 


Figure  V-41:  Spectral  Comparison,  Sine/Cosine  Method  Controlling  Harmonics 
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c)  Clear  Box,  Adaptive  Basis  Method 

The  same  two  cases  of  multiple  frequency  disturbances  are  tested  using 
the  Adaptive  Basis  Method  of  the  Clear  Box  algorithm.  For  the  case  of  two  distinct 
disturbances  the  six  sensor  outputs  are  shown  in  Figure  V-42,  and  the  before/after 
spectral  content  of  the  strut  #1  sensor  is  shown  in  Figure  V-43.  Note  there  is  still  a  small 
amount  of  energy  at  the  two  disturbance  frequencies.  This  is  likely  due  to  the  fact  that 
there  are  not  as  many  “redundant  shifts”  available  since  there  are  now  two  disturbances 
present  (discussed  further  in  Chapter  VI).  As  in  the  single,  static  frequency  case  the  use 
of  a  forgetting  factor  equal  to  1.0  helps  improve  performance  when  the  disturbance 
frequencies  are  not  varying  with  time. 
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Figure  V-42:  Adaptive  Basis  Method  Sensor  Outputs,  2  Static  Disturbances 
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Figure  V-43:  Adaptive  Basis  Method  Spectral  Comparison,  2  Static  Disturbances 


The  case  of  harmonic  disturbances  was  also  tested  using  the  Adaptive 
Basis  Method.  In  this  case  a  fundamental  frequency  of  105  Hz  is  chosen,  which 
generates  harmonics  at  210  Hz  and  315  Hz.  The  210  Hz  disturbance  is  not  very  strong, 
and  in  fact  the  controller  causes  slight  amplification  at  this  frequency,  but  there  is  clear 
reduction  of  the  energy  present  in  the  fundamental  disturbance  and  the  harmonic  at  315 
Hz,  as  seen  in  Figure  V-44. 
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Figure  V-44:  Spectral  Comparison,  Adaptive  Basis  Method  Controlling  Harmonics 


D.  REJECTING  TIME- VARYING  DISTURBANCES 

Now  that  all  three  controllers  have  been  tested  against  disturbances  that  do  not 
vary  with  time,  we  move  our  attention  to  the  case  of  time-varying  disturbances. 

1.  Single,  Time-Varying  Disturbance  Frequency 

To  test  the  controllers  against  a  single,  time-varying  disturbance  frequency  a 
standard  frequency  variation  profile  is  used.  This  profile,  shown  in  Figure  V-45,  starts  at 
120  Hz  and  is  held  constant  for  1  second.  Then  the  frequency  ramps  up  at  a  “rapid”  rate 
of  2  Hz/sec  for  four  seconds,  and  is  held  constant  at  128  Hz  for  one  second.  Then  the 
frequency  ramps  down  at  a  “slow”  0.1  Hz/sec  for  four  more  seconds,  and  is  finally  held 
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constant  at  127.6  Hz  for  two  seconds  to  complete  the  experiment.  In  all  cases  the 
controller  is  activated  at  0.5  seconds. 


Figure  V-45:  Frequency  Variation  Profile,  Single  Disturbance 

a )  Multiple  Error  LMS 

With  knowledge  of  the  exact  disturbance  frequency,  provided  by  the  x(k) 
signal,  the  Multiple  Error  LMS  algorithm  performs  quite  well  over  the  course  of  the 
frequency  variation  profile,  as  shown  in  Figure  V-46.  Only  a  slight  drop  in  performance 
is  noted  during  the  rapid  frequency  variation  phase. 
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Figure  V-46:  Multiple  Error  LMS  Sensor  Outputs,  1  Varying  Frequency 

b )  Clear  Box,  Sine/Cosine  Method 

The  Sine/Cosine  controller’s  performance  during  the  frequency  variation 
profile  is  shown  in  Figure  V-47.  In  the  UQP  implementation  of  the  Sine/Cosine  Method 
the  disturbance  frequencies  are  updated  once  per  second  using  batch  processing  of  the 
most  recent  rj(k)  time  history.  Thus,  as  the  true  disturbance  frequency  strays  from  the 
estimated  frequency  the  performance  begins  to  degrade.  The  coefficient  cycling,  shown 
in  Figure  V-48  for  the  strut  #1  coefficients,  is  unable  to  compensate  for  the  frequency 
variation  during  the  rapid  ramp-up  phase.  In  Section  C.l.b)  starting  on  page  106,  it  was 
shown  how  the  use  of  a  smaller  forgetting  factor  improves  performance  when  the 
frequency  estimate  is  incorrect.  Unfortunately,  use  of  a  forgetting  factor  below  0.9 
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during  the  frequency  variation  profile  resulted  in  excessive  noise  in  the  control 
coefficients  and  led  to  instability.  The  results  in  the  two  figures  below  are  obtained  using 
a  forgetting  factor  of  0.95. 
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Figure  V-47:  Sine/Cosine  Method  Sensor  Outputs,  1  Varying  Frequency 
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Figure  V-48:  Sine/Cosine  Method  Strut  #1  Coefficients,  1  Varying  Frequency 


c )  Clear  Box,  Adaptive  Basis  Method 

Finally,  the  frequency  variation  profile  is  used  while  controlling  the  UQP 
with  the  Adaptive  Basis  Method.  Again,  this  controller  does  not  need  to  estimate  the 
disturbance  frequencies  since  the  information  is  present  in  the  t](k)  signal.  The 
performance  is  very  good  during  the  entire  profile,  as  shown  in  Figure  V-49.  Note  that 
this  performance  is  obtained  without  the  use  of  a  sensor-provided  reference  signal. 
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Figure  V-49:  Adaptive  Basis  Method  Sensor  Outputs,  1  Varying  Frequency 


2.  Multiple  Time-Varying  Disturbance  Frequencies 

The  next  set  of  experiments  involves  the  use  of  two  time-varying  frequencies. 
The  variation  profile  is  shown  in  Figure  V-50.  The  first  frequency  starts  at  140  Hz  and 
ramps  up  at  a  rate  of  0.5  Hz/sec.  The  second  starts  at  144  Hz  and  ramps  down  at  0.5 
Hz/sec.  The  two  frequencies  “converge”  at  142  Hz  at  four  seconds  into  the  experiment 
(which  lasts  for  a  total  of  nine  seconds).  The  two  frequencies  combine  to  form  a 
disturbance  that  pulsates;  more  slowly  as  the  frequencies  converge,  and  more  rapidly  as 
they  diverge.  When  they  cross  at  142  Hz  they  appear  to  be  a  single  disturbance. 
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Figure  V-50:  Frequency  Variation  Profile,  2  Varying  Disturbances 


a)  Multiple  Error  LMS 

The  Multiple  Error  LMS  algorithm  is  tested  first  against  the  two  time- 
varying  disturbances,  and  the  six  sensor  outputs  and  the  strut  #1  control  coefficients  are 
shown  in  Figure  V-51  and  Figure  V-52,  respectively.  The  pulsating  nature  of  the 
disturbances  results  in  short  periods  of  degraded  performance  where  the  coefficients 
cannot  adapt  quickly  enough  to  keep  performance  at  an  optimum.  The  adaptation  rate 
used  for  this  experiment  is  0.01,  which  can  be  tuned  to  a  more  aggressive  value  for  better 
performance,  but  at  the  risk  of  instability.  Recall  from  earlier  experiments  that  higher 
frequency  disturbances  cause  larger  r2  levels,  resulting  in  instability  at  lower  values  of 
the  adaptation  rate. 
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Figure  V-51:  Multiple  Error  LMS  Sensor  Outputs,  2  Varying  Disturbances 


Figure  V-52:  Multiple  Error  LMS  Control  Coefficients,  2  Varying  Disturbances 
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b)  Clear  Box,  Sine/Cosine  Method 

Next,  the  Sine/Cosine  Method  is  tested  against  the  two  time-varying 


disturbances.  The  resulting  sensor  outputs  and  control  coefficient  histories  are  shown  in 
Figure  V-53  and  Figure  V-54,  respectively,  and  show  that  the  controller  has  difficulty 
maintaining  optimal  performance.  The  forgetting  factor  is  again  key  in  improving 
performance  since  the  true  frequencies  are  constantly  drifting  from  the  frequency 
estimates  (updated  once  per  second).  In  this  experiment  a  forgetting  factor  of  0.99  is 
used.  A  lower  value  improves  performance  but  instability  is  observed  for  values  below 
0.95.  During  the  period  from  5.5  to  6.5  seconds  the  Host  PC  frequency  estimation  code 
is  unable  to  detect  a  mode  in  the  disturbance  effect  signal  that  has  a  damping  ratio  below 
the  pre-set  threshold,  resulting  in  all  control  coefficients  being  set  to  zero,  and 
performance  returning  to  an  uncontrolled  state. 
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Figure  V-54:  Sine/Cosine  Method  Control  Coefficients,  2  Varying  Disturbances 
c)  Clear  Box,  Adaptive  Basis  Method 

Finally,  the  Adaptive  Basis  Method  is  tested  against  the  two  time-varying 
disturbances.  The  six  sensor  outputs  and  the  strut  #1  control  coefficients  are  shown  in 
Figure  V-55  and  Figure  V-56,  respectively,  and  show  relatively  good  performance  with  a 
pulsating  nature  similar  to  that  observed  with  the  Multiple  Error  LMS  controller.  The 
forgetting  factor  used  in  this  experiment  is  0.99  (the  same  as  the  Sine/Cosine  Method’s 
experiment).  Performance  is  not  affected  greatly  by  using  a  lower  value,  and  stability  is 
also  not  affected  by  a  lower  forgetting  factor,  in  contrast  to  the  Sine/Cosine  Method 
which  is  sporadically  unstable  for  values  below  0.95. 
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Figure  V-55:  Adaptive  Basis  Method  Sensor  Outputs,  2  Varying  Disturbances 


Figure  V-56:  Adaptive  Basis  Method  Control  Coefficients,  2  Varying  Disturbances 
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E.  THE  CASE  OF  AN  UNCONTROLLABLE  MODE 


One  of  the  primary  advantages  of  the  Clear  Box  algorithm  is  the  ability  to 
selectively  control  disturbances.  This  is  important  in  the  case  where  it  is  undesirable  to 
attempt  control  of  an  uncontrollable  or  weakly  controllable  mode,  since  actuator 
saturation  and  instability  may  result.  The  Multiple  Error  LMS  algorithm  is,  in  general, 
not  capable  of  selective  disturbance  control. 

The  experiments  in  this  section  show  that  both  methods  of  the  Clear  Box 
algorithm  allow  selective  disturbance  control.  The  capability  is  inherent  in  the 
Sine/Cosine  Method,  but  is  also  shown  with  the  Adaptive  Basis  Method  through  the  use 
of  filtering  of  the  disturbance  effect  signal. 

1.  Clear  Box,  Sine/Cosine  Method 

The  experiment  used  to  demonstrate  selective  disturbance  control  employs  two 
static  disturbances  at  110  Hz  and  140  Hz.  Since  the  highly  damped  UQP  system  does  not 
have  any  modes  that  are  “uncontrollable”  in  the  frequency  range  of  interest,  it  is 
necessary  to  artificially  designate  the  140  Hz  disturbance  as  being  uncontrollable. 

The  results  are  shown  in  the  three  figures  that  follow.  Figure  V-57  shows  The 
sensor  outputs  for  all  six  struts.  Before  the  controller  is  initiated  the  outputs  contain  the 
effects  of  both  disturbances.  In  the  disturbance  identification  portion  of  the  code,  logic  is 
implemented  that  “de-selects”  any  identified  disturbance  that  is  close  to  140  Hz.  This 
approach  is  valid  for  a  system  with  time-invariant  dynamics,  but  in  general  (for  systems 
with  time-varying  dynamics)  the  magnitude  of  the  required  control  action  can  be 
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analyzed  to  determine  if  a  particular  disturbance  is,  in  fact,  “uncontrollable”.  Figure 
V-58  simply  shows  a  close  up  view  of  the  sensor  outputs,  showing  that  the  signal  that 
remains  is  a  140  Hz  signal.  The  before/after  spectral  comparison  is  shown  in  Figure 
V-59,  indicating  control  action  effectively  removes  the  disturbance  at  1 10  Hz,  but  does 
nothing  at  140  Hz.  There  are  no  significant  adverse  effects  at  any  other  frequencies. 


Figure  V-57:  Sine/Cosine  Method  Sensor  Outputs,  Selective  Disturbance  Control 
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Figure  V-58:  Sine/Cosine  Method  Sensor  Outputs  (Zoom  in) 


Figure  V-59:  Sine/Cosine  Method  Spectral  Comparison,  Selective  Control 
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2.  Clear  Box,  Adaptive  Basis  Method 

The  same  experiment  described  above  for  the  Sine/Cosine  Method  is  tested  using 
the  Adaptive  Basis  Method.  A  digital  filter,  with  frequency  response  shown  in  Figure 
V-60,  is  employed  to  filter  the  disturbance  effect  signal,  and  effectively  eliminate  the 
information  at  the  140  Hz  frequency.  Thus,  the  time  shifted  disturbance  effect  basis 
functions  are  unable  to  control  the  disturbance  at  140  Hz.  The  results  are  shown  in  the 
three  figures  that  follow.  The  before/after  spectral  comparison  in  Figure  V-63  shows 
effective  control  of  the  110  Hz  disturbance,  but  no  control  of  the  140  Hz  disturbance. 
Some  minor  effects  at  other  frequencies  are  noted. 


Figure  V-60:  2nd  Order  Butterworth  Notch  Filter  for  Selective  Control 
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Frequency  [Hz] 


Figure  V-63:  Adaptive  Basis  Method  Spectral  Comparison,  Selective  Control 
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VL  DISCUSSION  OF  RESULTS 


In  this  chapter  the  performance  characteristics  of  the  Multiple  Error  LMS,  Clear 
Box  Sine/Cosine,  and  Clear  Box  Adaptive  Basis  controllers  are  compared  across  the 
various  experiments  performed  in  Chapter  V.  The  comparisons  are  divided  into  the  two 
broad  categories  of  "performance"  and  "implementation  issues". 


A.  PERFORMANCE 

1.  General 

The  experiments  that  demonstrate  control  of  a  single  static  disturbance  (conducted 
in  Chapter  V)  reveal  several  key  characteristics  of  the  three  controllers.  All  three  are  able 
to  control  the  disturbance  to  the  noise  level  across  the  entire  selected  control  bandwidth 
(20-300  Hz).  The  three  controllers  also  demonstrate  that  they  impart  very  little  energy  to 
the  system  at  frequencies  other  than  the  disturbance  frequency.  This  is  especially  true  in 
the  case  of  the  Sine/Cosine  Method  since  the  control  signal  consists  of  pure  sine  &  cosine 
functions  at  the  estimated  disturbance  frequencies. 

The  experiments  with  two  static  disturbances  reveal  that  the  Multiple  Error  LMS 
Algorithm  and  the  Clear  Box  Sine/Cosine  Method  achieve  slightly  better  performance 
than  the  Clear  Box  Adaptive  Basis  Method.  The  Adaptive  Basis  Method  increases  the 
energy  at  frequencies  other  than  the  disturbance  frequency  (Figure  V-43),  indicating  that 
these  unintended  frequency  components  are  present  in  the  disturbance  effect  signal  and 
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thus  in  the  basis  functions.  The  available  processing  power  only  allows  a  maximum  of 
N  =  6  when  using  the  Adaptive  Basis  Method,  which  limits  control  to  three  frequencies 
(or  two  frequencies  and  an  offset  bias).  The  existence  of  additional  low-level  frequency 
components  is  the  most  likely  reason  the  controller  is  unable  to  completely  cancel  the  two 
static  disturbances.  A  36  dB  drop  is  achieved,  in  comparison  with  the  40  dB  drop  of  the 
Sine/Cosine  Method. 

An  advantage  of  the  Clear  Box  Algorithm  is  the  inherent  ability  to  control 
unexpected  disturbances  and  harmonics.  Again,  the  disturbance  effect  signal  reveals  all 
system  outputs  that  are  not  predicted  by  the  past  input/output  data  as  applied  to  the 
disturbance-free  system  model  (Eq.s  (2-60)  and  (2-62)).  These  disturbances  are 
controlled  without  difficulty  since  the  two  Clear  Box  methods  use  the  disturbance  effect 
signal  as  either  a  source  for  the  disturbance  frequency  estimates  (Sine/Cosine  Method)  or 
as  a  basis  function  source  (Adaptive  Basis  Method).  In  contrast,  controlling  harmonics 
with  the  Multiple  Error  LMS  Algorithm  requires  prior  knowledge  of  the  number  of 
harmonics  that  need  control.  Periodic  signals  at  the  harmonic  frequencies  can  then  be 
artificially  generated  and  added  to  the  original  reference  signal  (from  which  the 
frequency  information  is  obtained).  This  process  is  computationally  inefficient  since  not 
all  disturbance  frequencies  have  harmonics  that  significantly  affect  the  system  output.  In 
addition,  unexpected  disturbances  may  not  be  controllable  with  the  Multiple  Error  LMS 
Algorithm  since  the  pre-selected  sensor  locations  may  yield  a  very  weak  signal. 

Experiments  controlling  time-varying  disturbance  frequencies  reveal  that  the 
Clear  Box  Sine/Cosine  Method  is  unable  to  control  rapidly  changing  disturbances. 
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Although  coefficient  cycling  allows  some  improvement  from  the  uncontrolled  condition, 
excessive  rates  of  change  cannot  be  compensated  for.  Typically  the  disturbances  present 
on  spacecraft  are  not  varying  rapidly  with  time,  with  the  exception  of  momentum  wheels 
that  often  change  rates  rapidly  during  slew  or  momentum  dumping  maneuvers.  However, 
the  general  applicability  of  the  Clear  Box  Algorithm  is  improved  if  it  can  control  rapidly 
changing  disturbances.  The  Adaptive  Basis  Method  addresses  this  need,  as  shown  in  the 
time-varying  disturbance  experiments  in  Chapter  V.  An  alternative  approach  is  to 
improve  the  Sine/Cosine  Method’s  ability  to  estimate  frequencies  accurately  (discussed 
in  Section  HD)  or  increase  the  rate  with  which  the  updated  frequencies  are  provided,  or  a 
combination  of  both. 

Selective  disturbance  control  is  a  feature  unique  to  the  Clear  Box  Algorithm,  and 
is  demonstrated  on  a  hypothetical  "uncontrollable  mode"  in  Chapter  V  using  both  Clear 
Box  methods.  The  Multiple  Error  LMS  Algorithm  does  not  provide  for  selective  mode 
control.  The  only  possible  modification  that  would  allow  a  degree  of  selective  control 
would  be  to  implement  a  comb  filter  for  the  reference  signal  x(k)  (frequencies  that  are 
de-selected  for  control  are  filtered  out  of  x(k) ).  This  implies  adding  some  form  of 
system  identification  capability  to  the  LMS  control  implementation.  Even  so,  the  only 
way  to  determine  which  frequencies  are  weakly  controllable  is  by  finding  the  system 
transmission  zeros.  There  is  no  capability  to  determine  what  portion  of  the  control  signal 
is  due  to  a  particular  frequency,  or  how  much  of  the  system  output  is  due  to  a  particular 
disturbance.  Without  this  information  the  disturbance  control  selections  cannot  be  made 
as  intelligently  as  with  Clear  Box. 
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2.  Tuning  Requirements 

A  drawback  of  the  Multiple  Error  LMS  Algorithm  is  that  tuning  of  the  adaptation 
rate  is  required  to  achieve  optimal  convergence  to  the  steady-state  filter  weight  solution. 
For  a  given  adaptation  rate,  instability  is  more  likely  as  the  disturbance  frequency 
increases,  and  convergence  is  slower  as  the  disturbance  frequency  decreases.  The 
adaptation  rate  must  be  chosen  conservatively  (i.e.,  a  low  rate)  to  guarantee  that  stability 
is  maintained,  and  thus  rapid  convergence  is  not  possible  for  every  disturbance. 

By  comparison,  the  Clear  Box  methods  require  relatively  little  tuning.  The 
forgetting  factor's  effect  on  performance  has  no  dependence  on  the  disturbance 
frequency.  For  the  Sine/Cosine  Method  a  forgetting  factor  of  0.95  yields  stable 
performance  (even  with  rapidly  varying  disturbances)  and  reduction  to  the  noise  level 
when  typical  frequency  estimation  errors  are  present.  The  Adaptive  Basis  Method 
performs  well  in  all  cases  (static  or  varying  disturbances)  when  a  forgetting  factor  of  0.99 
is  used. 


3.  Processing  Requirements 

A  quantitative  comparison  of  the  processing  required  for  the  investigated 
algorithms  reveals  the  relative  efficiency  of  each.  There  is  no  way  to  directly  compare 
the  Multiple  Error  LMS  Algorithm  to  the  Clear  Box  Algorithm  since  one  uses  an  FIR 
filter  model  and  the  other  uses  an  ARX  model.  Although  there  is  no  way  to  use  the  same 
“model  order”,  it  is  revealing  to  note  that  using  p  =  10  for  the  Clear  Box  Algorithm,  and 
J  =  20  for  the  Multiple  Error  LMS  Algorithm  both  yield  system  models  with  720 
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coefficients  (for  6  inputs  and  6  outputs).  This  may  be  misleading  for  lightly-damped 
systems,  however,  since  the  FIR  model  order  would  need  to  be  much  larger  to  match  the 
accuracy  that  is  possible  with  a  relatively  low  order  ARX  model. 

The  tables  below  indicate  the  percentage  of  available  processing  power  used  by 
each  algorithm  for  various  model  orders,  and  employing  different  numbers  of  filter 
weights  or  control  coefficients.  The  cases  that  offer  an  approximate  comparison 
(assuming  a  well-damped  system)  are  circled. 


\ 

Number  of  Filter  Weights  per  strut,  I 

5 

10 

15 

FIR  Model  Order,  J 

20 

© 

31.7 

43.4 

30 

23.6 

44.2 

61.3 

40 

30.5 

57.4 

80.0 

Table  VI-1:  Multiple  Error  LMS  Algorithm,  Processing  Required  (%) 
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Number  of  Frequencies  Controlled,/ 

1 

2 

3 

Model  Order,  p 

10 

19.3 

41.1 

15 

22.5 

45.8 

79.0 

20 

25.3 

50.5 

85.0 

Table  VI-2:  Clear  Box  Sine/Cosine  Method,  Processing  Required  (%) 


\ 

Number  of  Basis  Functions,  N 

2 

4 

6 

Model  Order,  p 

10 

21.2 

44.2 

15 

24.6 

51.5 

84.3 

20 

27.3 

55.8 

92.9 

Table  VI-3:  Clear  Box  Adaptive  Basis  Method,  Processing  Required  (%) 


4.  Stability 

The  experiments  performed  in  Chapter  V  demonstrate  that  the  three  algorithms 
are  sufficiently  stable  while  operating  under  normal  conditions,  and  adapt  to  situations 
where  the  plant  model  is  altered  by  the  addition  of  mass  to  the  top  of  the  platform. 
Impulsive  disturbances  to  the  platform  are  handled  very  well  by  the  Multiple  Error  LMS 
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Algorithm,  and  also  by  the  Clear  Box  Sine/Cosine  Method,  with  coefficients  rapidly 
returning  to  pre-impulse  values.  While  the  Clear  Box  Adaptive  Basis  Method  can  handle 
small  impulsive  disturbances,  very  large  disturbances  tend  to  cause  instability.  The 
immediate  presence  of  the  disturbance  information  in  the  basis  functions  of  the  controller 
results  in  an  attempt  by  the  control  coefficients  to  cancel  the  disturbance,  and  the  PZT 
actuators  are  quickly  saturated  due  to  their  small  displacement  capability  (50  microns). 

The  extremely  nonlinear  nature  of  the  Adaptive  Basis  Method  hinders  analytical 
proof  of  the  algorithm’s  stability  for  cases  other  than  single  disturbance  frequencies  with 
slow  adaptation  on  SISO  systems.  The  Clear  Box  Sine/Cosine  Method  was  shown  to  be 
stable  under  such  assumptions  in  the  presence  of  small  perturbations  in  the  system  model 
parameters  [Ref.  108],  and  has  an  effective  phase  margin  of  ±90  degrees.  The  single 
channel  Filtered-*  LMS  Algorithm  has  been  shown  to  be  stable  theoretically  (also  with  a 
±90  degree  phase  margin)  [Ref.s  109, 1 10],  while  the  Multiple  Error  LMS  Algorithm  has 
only  been  theoretically  proven  stable  for  restricted  disturbance  cases  [Ref.s  111,112]. 
Despite  this  lack  of  proven  stability  (for  the  general  case)  adaptive  feedforward 
algorithms  have  been  successfully  applied  to  many  applications  with  good  observed 
stability,  even  using  rapid  adaptation  rates. 


B.  IMPLEMENTATION  ISSUES 

The  UQP  system  is  a  highly  damped  system  with  dynamics  that  do  not  vary 
significantly  with  time.  Implementing  adaptive  feedforward  control  for  a  different 
application  first  requires  a  determination  of  several  characteristics  of  the  system  and  the 
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expected  disturbances.  The  factors  affecting  controller  selection  for  a  given  application 
include;  1)  the  nature  of  the  system  dynamics,  2)  the  nature  of  the  disturbances,  3)  the 
availability  of  processing  resources,  4)  the  cost  associated  with  additional  sensors,  5)  the 
control  authority  of  the  selected  actuators,  and  6)  the  importance  of  reliability  and  fault 
tolerance. 


1.  System  Dynamics  Considerations 

If  the  system  dynamics  (from  actuator  input  to  sensor  output)  vary  with  time,  then 
periodic  re-identification  is  required  to  maintain  optimal  performance.  The  system 
identification  approach  inherent  in  the  Clear  Box  Algorithm  is  well-suited  to  such 
systems,  allowing  operation  without  any  prior  knowledge  of  the  nature  of  the  physical 
plant. 

If  the  system  is  lightly  damped  then  the  required  model  order  is  much  less  when 
using  an  ARX  model  (as  with  Clear  Box)  than  with  an  FIR  filter  model  (as  with  LMS 
algorithms).  A  lower  model  order,  in  general,  translates  into  reduced  processing 
requirements.  Typically,  lightly  damped  systems  have  transmission  zeros  that  translate 
into  weakly  controllable  modes.  The  required  control  signal  at  these  frequencies  is  very 
large  since  the  low  gain  of  the  system  must  be  compensated  for.  Since  it  is  desirable  to 
use  light  weight  (generally  less  capable)  actuators  for  space  applications,  a  large  control 
signal  will  likely  cause  actuator  saturation,  leading  to  possible  system  instability.  The 
selective  control  capability  unique  to  the  Clear  Box  Algorithm  is  indispensable  in  such 
cases.  Equations  (2-66)  and  (2-85)  are  used  to  determine  the  magnitude  of  the  required 
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control  signal,  and  the  effect  of  each  disturbance  on  the  system  output,  respectively.  If 
control  authority  is  limited,  logic  statements  are  used  to  de-select  disturbances  for  control 
if  the  magnitude  of  the  required  control  signal  is  too  large  (thus  preventing  actuator 
saturation)  or  if  the  impact  on  the  system  output  is  very  small  (preventing  inefficient  use 
of  processor  resources  on  insignificant  disturbances). 

2.  Disturbance  Considerations 

If  it  is  impossible  (or  undesirable)  to  turn  off  disturbance  sources  when 
performing  system  identification  the  resulting  model  will  be  disturbance-corrupted.  In 
such  cases  the  Clear  Box  Algorithm  is  the  only  alternative  for  obtaining  a  disturbance- 
free  system  model.  In  fact,  in  50%  of  all  cases,  finite  length  data  records  result  in 
identified  disturbance  modes  having  slightly  negative  damping  ratios.  This  results  in  the 
identified  model  being  unstable,  and  unusable  for  control. 

For  the  case  of  a  system  with  time-invariant  dynamics  (such  as  the  UQP),  periodic 
disturbances  that  have  slowly  varying  frequencies  (i.e.,  change  less  than  0.1  Hz  per 
second)  can  be  controlled  equally  well  by  the  three  controllers  implemented  in  this 
research.  A  slight  edge  is  given  to  the  Clear  Box  Sine/Cosine  Method  due  to  its  complete 
lack  of  negative  impact  at  frequencies  other  than  those  of  the  controlled  disturbances. 

Rapidly  varying  disturbances  are  more  effectively  controlled  by  the  methods  that 
do  not  require  estimates  of  the  disturbance  frequencies  (the  Multiple  Error  LMS 
Algorithm  and  the  new  Clear  Box  Adaptive  Basis  Method).  However,  the  performance 
of  the  Sine/Cosine  Method  against  rapidly  time-varying  disturbances  could  be  greatly 
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improved  by  implementing  a  polynomial  curve  fit  for  the  time-history  of  each 
disturbance’s  frequency.  This  would  allow  a  more  accurate  estimate  of  the  frequency  at 
the  time  steps  between  frequency  updates.  More  efficient  processing  techniques  would 
also  shorten  the  time  required  between  updates. 

3.  Coding  and  Processing 

The  Multiple  Error  LMS  Algorithm  requires  the  least  amount  of  coding,  mainly 
due  to  the  fact  that  it  does  not  require  recursive  estimation  of  its  control  parameters. 
Instead  it  uses  a  simple  filter  weight  update  consisting  of  an  adaptation  rate  “gain”,  and 
the  product  of  the  error  signals  and  the  filtered  reference  signal,  as  shown  in  Eq.  (2-22). 
The  result  is  that  the  Multiple  Error  LMS  Algorithm  requires  the  least  amount  of 
processing  power. 

The  computational  requirements  of  the  two  Clear  Box  methods  are  similar,  but 
there  are  some  differences  worthy  of  discussion.  The  number  of  basis  functions  used  in 
the  Sine/Cosine  Method  is  always  twice  the  number  of  disturbance  frequencies  that  have 
been  identified  and  selected  for  control,  since  a  sine/cosine  pair  is  assigned  to  each 
estimated  disturbance  frequency.  Thus,  if  new  disturbances  emerge  the  required 
processing  power  will  increase  as  sine/cosine  pairs  are  added.  For  the  Adaptive  Basis 
Method,  the  number  of  basis  functions  N  is  selected  ahead  of  time,  so  an  upper  bound  on 
the  number  of  disturbance  frequencies  /  is  required  to  assess  the  correct  value  of  N 
according  to  Eq.  (2-87).  For  this  reason,  the  processing  requirements  of  the  Adaptive 
Basis  Method  are  constant,  and  do  not  depend  on  the  number  of  disturbances  present. 
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The  Sine/Cosine  Method  is  the  only  controller  (of  the  three)  that  requires 
estimates  of  the  disturbance  frequencies  (a  process  that  must  be  done  in  batch  mode  using 
modal  decomposition  and  logic  statements).  This  process,  performed  by  the  host  PC 
during  the  UQP  experiments,  represents  additional  computation  required  for  this  method. 

For  systems  with  time- varying  dynamics,  or  to  add  fault  tolerance  to  the  system, 
re-identification  of  the  system  model  is  required  periodically.  To  perform  disturbance- 
free  system  identification  using  disturbance-corrupted  data  (via  the  Clear  Box  Algorithm) 
requires  selective  elimination  of  disturbance  modes  using  logic  statements,  which  must 
also  be  done  in  batch  mode  using  an  additional  processor.  Again,  this  function  is 
performed  by  the  host  PC  for  the  UQP  experiment. 

As  space  qualified  processors  become  more  capable  (and  require  less  power, 
weight,  and  volume)  the  additional  processing  burden  required  by  the  Clear  Box 
Algorithm  will  be  less  of  a  consideration.  The  added  capabilities  (identification  in  the 
presence  of  disturbances,  control  of  time-varying  systems,  and  selective  mode  control) 
represent  features  not  included  in  the  Multiple  Error  LMS  Algorithm.  Thus  the  added 
processing  required  by  the  Clear  Box  Algorithm  is  not  without  added  benefit. 

4.  Cost  Associated  with  Additional  Sensors 

A  primary  advantage  of  the  Clear  Box  Algorithm  is  the  ability  to  control  any 
periodic  disturbance  (including  unexpected  disturbances  and  harmonics)  without 
requiring  an  additional  sensor  to  provide  a  disturbance-correlated  signal.  Addition  of 
sensors  to  provide  this  signal  for  the  Multiple  Error  LMS  Algorithm  is  accompanied  by 
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the  addition  of  power  supplies,  signal  conditioning,  wiring,  a  signal  combiner  (to  provide 
a  single  disturbance  signal  from  all  of  the  sensors)  and  mounting  hardware.  Also  to  be 
considered  are  the  additional  weight,  volume,  and  integration  &  test  costs  associated  with 
these  components. 

5.  Reliability  and  Fault  Tolerance 

The  ability  to  rapidly  re-accomplish  system  identification  is  important  for 
handling  unexpected  situations  such  as  a  mechanical  failure  of  the  system.  For  example, 
if  the  UQP  experienced  a  strut  actuator  failure  it  is  possible  the  system  model  for  the 
remaining  struts  would  be  affected.  Since  the  Clear  Box  Algorithm  requires  no 
knowledge  of  the  system  dynamics,  the  next  system  identification  performed  on  the 
system  would  account  for  any  changes  induced  by  the  failure.  For  the  case  of  the  UQP 
the  strut  with  the  failed  actuator  would  still  offer  passive  isolation  capability.  In  this 
manner,  the  system  identification  approach  to  the  control  problem  adds  a  degree  of  fault 
tolerance  to  the  controller  that  is  not  present  with  the  LMS  Algorithm. 
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VII.  CONCLUSIONS 


The  Multiple  Error  LMS  Algorithm  is  widely  accepted  for  use  with  MIMO 
systems  that  require  vibration  control/disturbance  rejection.  However,  the  need  to  supply 
the  algorithm  with  a  disturbance  correlated  signal  is  undesirable  for  space  applications 
since  additional  sensor  hardware  and  wiring  is  required.  Also,  traditional  methods  allow 
no  capability  to  selectively  assign  control  action  to  specific  disturbances,  and  thus  the 
actuators  must  be  sized  to  handle  control  of  all  expected  disturbances  simultaneously. 

The  Clear  Box  Algorithm  approaches  active  disturbance  rejection  from  a  system 
identification  perspective,  allowing  intelligent  operation  in  an  information-rich 
environment.  The  algorithm  can  handle  systems  with  time-varying  dynamics,  does  not 
require  a  measured  disturbance-correlated  signal,  and  can  handle  unanticipated 
disturbances  and  harmonics.  The  selective  disturbance  control  feature  of  the  Clear  Box 
Algorithm  allows  intelligent  assignment  of  actuator  resources  based  on  available 
information  about  the  size  of  the  required  control  signal  and  the  impact  of  each 
disturbance  on  the  system  output.  Thus,  smaller  light-weight  actuators  can  be  used  since 
selective  control  can  prevent  actuator  saturation.  Fault  tolerance  and  adaptability  are 
enhanced  by  the  Clear  Box  Algorithm  through  the  use  of  frequent  system  identification. 

The  Clear  Box  Sine/Cosine  Method  requires  estimates  of  the  disturbance 
frequencies,  and  performance  suffers  if  the  frequencies  drift  significantly  before  the  next 
update  is  available.  Efficient  processing  techniques  can  reduce  the  time  between  updates, 
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but  eliminating  the  need  for  frequency  estimation  is  preferable.  A  new  Adaptive  Basis 
Method  is  presented  that  uses  the  disturbance  effect  signal  directly  as  a  computed  basis 
for  the  control  signal.  This  new  method  addresses  the  problem  of  controlling  rapidly 
varying  frequencies  (since  frequency  estimates  are  not  required),  and  maintains  the 
attractive  features  associated  with  the  Clear  Box  Algorithm. 

Extensive  experiments  employing  the  three  control  techniques  on  the  Ultra  Quiet 
Platform  demonstrate  the  attractiveness  of  Clear  Box  methods  for  active  vibration 
isolation  and  disturbance  rejection.  Performance  meets  or  exceeds  that  of  the  Multiple 
Error  LMS  Algorithm  while  requiring  neither  prior  knowledge  of  the  system  dynamics 
nor  a  measured  disturbance-correlated  signal. 


A.  CONTRIBUTIONS 

The  Multiple  Error  LMS  algorithm  has  been  demonstrated  in  many  different 
applications  of  noise  and  vibration  control.  The  Clear  Box  algorithm,  however,  has  only 
been  implemented  using  the  Sine/Cosine  Method  for  purposes  of  controlling  structural 
vibrations  on  a  truss  [Ref.  113],  Thus,  this  is  the  first  application  of  Clear  Box  on  a 
vibration  isolation  platform,  and  the  first  experimental  implementation  using  greater  than 
two  inputs  and  two  outputs.  The  utility  of  Clear  Box  for  spacecraft  vibration  isolation 
comes  from  eliminating  the  requirement  for  additional  sensors,  allowing  the  use  of  lighter 
weight  actuators,  and  adding  a  greater  degree  of  fault  tolerance  and  adaptability  by 
employing  frequent  system  identification. 
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The  Sine/Cosine  Method’s  coefficient  cycling  (when  frequency  estimates  are 
incorrect)  had  been  observed,  but  not  explained.  The  theoretical  prediction  of  this 
cycling  is  offered  for  the  first  time  in  this  dissertation. 

The  Adaptive  Basis  Method  was  developed  during  the  course  of  this  research.  It 
has  the  capability  to  control  rapidly  varying  disturbances  to  a  degree  not  currently 
possible  with  the  Sine/Cosine  Method,  and  is  still  capable  of  selective  disturbance 
control.  Thus,  all  of  the  attractive  features  of  the  Clear  Box  Algorithm  are  retained  while 
adding  an  improved  capability  to  control  rapidly  varying  frequencies. 

The  data  acquisition  and  control  system  for  the  UQP  experiment  was  also 
implemented  during  the  course  of  this  research.  All  coding  was  developed  so  that  the 
user  can  easily  change  the  number  of  inputs,  outputs,  and  sample  rate.  Thus,  either  the 
LMS  or  Clear  Box  controllers  could  be  easily  adapted  and  applied  to  other  experiments. 
The  equipment  remains  in  the  Spacecraft  R&D  Center’s  Smart  Structures  Laboratory  at 
the  Naval  Postgraduate  School  for  follow-on  work. 


B.  RECOMMENDATIONS  FOR  FURTHER  STUDY 

The  effectiveness  of  the  Adaptive  Basis  Method  could  be  improved  by  adding 

more  shifted  basis  functions  (i.e.  N>  6).  This  could  be  verified  by  adding  additional 
processing  to  the  UQP  control  hardware,  or  by  implementing  the  controller  on  a  system 
with  fewer  inputs  &  outputs.  It  is  quite  possible  that  any  periodicity  to  the  sensor  “noise” 
would  be  controlled  by  using  more  shifted  basis  functions,  allowing  consistent  control  to 
a  level  below  the  typical  RMS  noise  levels  encountered  in  this  research. 
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The  instability  induced  by  large  impulsive  disturbances  while  using  the  Adaptive 
Basis  Method  could  be  controlled  through  logic  statements  that  effectively  turn  off  the 
adaptation  of  the  basis  functions  if  a  large  disturbance  is  sensed.  Typically  such 
disturbances  are  of  very  short  duration,  and  the  impact  to  performance  would  be  minimal. 
Implementation  of  such  a  “smart  switch”  would  improve  the  overall  stability  properties 
of  the  controller. 

An  alternative  solution  to  the  rapidly  varying  frequency  problem  is  to 
significantly  improve  the  frequency  estimation  accuracy  of  the  Sine/Cosine  Method. 
Implementing  polynomial  curves  fit  to  past  frequency  estimates  would  allow  more 
accurate  extrapolation  of  the  disturbance  frequencies  to  the  time  steps  prior  to  the  next 
update.  This  would  allow  more  precise  control  of  rapidly  varying  disturbances,  while 
maintaining  the  desirable  performance  of  the  Sine/Cosine  Method. 

Since  the  Clear  Box  Algorithm  provides  an  accurate  system  dynamics  model,  this 
model  could  be  used  to  implement  a  feedback  controller  for  broadband  disturbances.  The 
two  controllers  working  together  would  provide  a  total  solution  for  disturbance  rejection, 
and  could  be  implemented  on  the  UQP  or  another  experimental  apparatus. 

An  ideal  adaptive  feedforward  controller  would  have  the  intelligence  of  the  Clear 
Box  Algorithm  and  the  computational  efficiency  of  the  LMS  Algorithm.  Creating  a 
“hybrid”  controller  is  possible  by  using  the  system  identification  information  from  Clear 
Box  to  improve  the  intelligence  of  the  LMS  Algorithm,  while  also  eliminating  the  need 
for  a  separate  sensor  to  provide  the  disturbance-correlated  signal.  This  would  be 
accomplished  by  using  the  calculated  disturbance  effect  signal  rj(k)  as  the  reference 
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signal  x(k ) .  Selective  disturbance  control  would  be  accomplished  through  filtering  of 
the  disturbance  effect  signal,  as  with  the  Adaptive  Basis  Method.  Preliminary  efforts  to 
accomplish  this  met  with  some  success,  but  more  work  is  required  to  refine  the  technique. 


165 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


166 


APPENDIX  A:  COMPUTER  CODE 


The  following  code  listings  are  provided  for  the  Clear  Box  Algorithm’s 
implementation  on  the  Ultra  Quiet  Platform.  Some  of  the  more  simple  subroutines  have 
been  omitted  for  brevity.  A  description  of  each  file  and  its  function  are  provided  below. 


File  Name 

Description 

Type 

Page 

# 

init_uqp.m 

Initializes  variables  needed  by  the  Clear  Box 

Algorithm 

M  file 
(script) 

168 

start_uqp.m 

Host  PC  supervisory  code  that  interfaces  with  the  user, 
controls  the  order  of  tasks  accomplished  by  the  DSP, 
and  initiates  system  and  disturbance  identification. 

M  file 
(script) 

171 

cbox_sys_id.m 

System  identification  code 

M  file 
(function) 

178 

eta2freq_id.m 

Frequency  identification  from  disturbance  effect  data 

M  file 
(function) 

181 

arx2ss.m 

Conversion  from  ARX  form  to  state-space  form 

M  file 
(function) 

183 

ss2modal.m 

Diagonalization  of  state-space  model 

M  file 
(function) 

184 

ss2arx.m 

Conversion  from  state-space  form  to  ARX  form 

M  file 
(function) 

186 

elim_dist.m 

Elimination  of  disturbances  from  corrupted  model 

M  file 
(function) 

188 

analyze_modes.m 

System  ID  mode  analysis  to  determine  which  modes 
are  disturbances  (used  for  subsequent  elimination  of 
disturbance  modes  from  the  system  model) 

M  file 
(function) 

189 

analyze_eta_model.m  1 

Frequency  ID  mode  analysis  to  determine  which 
modes  are  disturbances  (used  for  controlling  the 
disturbances  using  the  Sine/Cosine  Method) 

M  file 
(function) 

191 

dist_gen.c 

Disturbance  generator 

C  code 
(S-function) 

192 

eta.c 

Disturbance  Effect  Calculation 

C  code 
(S-function) 

198 

u_ff.c 

Feedforward  Control  Calculation  -  Sine/Cosine 

Method 

C  code 
(S-function) 

202 

u_ff_eta.c 

Feedforward  Control  Calculation  -  Adaptive  Basis 
Method 

C  code 
(S-function) 

214 

Table  A-l:  Index  of  Computer  Code  Listings 
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VARIABLE  INITIALIZATION 


%  init_uqp.m  Stephen  Edwards  10  May  99 

% 

%  This  script  file  initializes  the  variables  and  parameters  needed 
%  for  the  UQP  control  experiment  "uqp.mdl" 

clear 

close  all  %  Closes  all  open  figure  windows 


%  Initialize  the  parameters  &  parameter  sizes  needed  for  the  S- 
Functions  that  calculate 

%  the  disturbance  effect  and  the  feedforward  control  signal (s). 


% 

%  P 
%  m 

%  q 

%  theta_bar 
% 

% 

% 

% 

% 

%  nfreq 
%  max„freq 
%  control_freqs 


=  Order  of  ARX  model 

=  Number  of  inputs  to  UQP  (actuators) 

=  Number  of  outputs  from  UQP  (sensors) 

=  Disturbance-free  ARX  model  coefficients 
=  [alpha_bar'  ;  beta_bar'] 

-  each  alpha_bar  "coefficient"  has  dimension  qxq 

-  each  beta__bar  "coefficient"  has  dimension  qxm 

-  there  are  p  of  each,  so  the  dimensions  of  theta 

are  [ (p*q  +  p*m)  x  q] 

=  The  number  of  freq's  to  be  controlled  currently 
=  The  max  possible  number  of  freq's  to  be  controlled 
=  Identified  frequencies  to  control 


fprintf(  * \n  \n  \n  Welcome  to  the  UQP  Control  Experiment...  \n  \a’) 
m  =input(  ’ \n  How  many  input  channels?  [eg.  6]:  '); 

q  =input(  ’ \n  How  many  output  channels?  [eg.  6] :  '  ); 

p  =input ( [ ' \n  What  value  for  p  (options  below)?',  ... 

' \n  [5,6,7,8,9,10,15,20,30  or  40]:  ']); 


%  Initialize  variables  for  Recursive  Least  Squares 
lambda  =0.999;  %  Forgetting  Factor 

p_init  =lel;  %  Initial  RLS  covariance 

%  Initialize  variables  for  u_ff.c 
nfreq  =0; 

max_f req  =3 ; 

control_freqs  = zeros (l,max_f req) ;  %  Initialize  to  zero 


%  Initialize  variables  for  u_f  f_eta__Nx.  c 


basis  =3 ; 

filt_order  =8; 

filt_alpha  =zeros (1, f ilt_order+l) ; 

filt_beta  =zeros (1 , f ilt_order+l) ; 


%  Strut  number  chosen  as  basis 
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%  Initialize  the  ARX  model  to  reference  values 
switch  p 
case  5 


load 
case  10 

mode 1 s \ cbox_p  0  5_1 0  s  ec 

theta__bar 

alpha_bar 

beta_bar 

load 
case  15 

mode  1  s  \  cbox_p  1 0_JL  0  s  ec 

theta_bar 

alpha_bar 

beta_bar 

load 
case  20 

models \cbox_j?15_10sec 

theta_bar 

alpha_bar 

beta__bar 

load 
case  25 

mode  1  s  \  cbox_p2  0__1 0  s  ec 

theta_bar 

alpha_bar 

beta__bar 

load 
case  30 

mode 1 s \ cbox_p2  5_1 0  s  ec 

theta_bar 

alpha_bar 

beta_bar 

load 
case  40 

models \cbox_p30_10sec 

theta_bar 

alpha„bar 

beta_bar 

load 

end 

mode 1 s \ cbox_p4  0_1 0  s  ec 

theta_bar 

alpha_bar 

beta_bar 

%  Initialize  the 
flag__update  =0 

flag_control  =0 

flag_noise  =0 

flag_disturb  =0 

f lag_dist_var  =0 

flag_reset  =0 

f 1 ag_wr ong_f  r eq=  0 

f lag_eta_f ilter=0 ; 

flag_archive  =  0; 


enable/disable  flags  (on=l,  off=0) 

%  Feedforward  coefficient  update 
%  Feedforward  control 
%  Excitation  white  noise 
%  Sinusoidal  disturbance (s) 

%  Time  variation  of  disturbance (s) 

%  Reset  for  controller  RLS  algorithm 
%  For  experiments  where  the  ID'd  freq.  Is 
deliberately  set  incorrectly 
%  For  eta-controller  experiments  where  not  all 
frequencies  are  controlled 
%  Archive  data  from  the  experiment  run 


%  Initialize  the  DC  offset  and  the  AC  limit.  The  UQP  actuators  should 

%  be  operated  in  the  0-100  Volt  region,  so  typically  an  offset  of  50 

%  Volts  is  used,  and  the  limit  on  the  AC  signal  should  be  set  to  +  /-  20 

%  Volts  at  most.  Note:  Levels  are  voltage  at  actuator  (after 

%  amplifier) .  The  gain  associated  with  the  D/A  converter  and  the 
%  amplifier  are  accounted  for  using  a  gain  block  in  the  "Control  Signal 
%  Processing"  Subsystem  in  uqp.mdl  (Simulink  Diagram) . 


DC_offset  =50.0; 

AC_limit  =10.0; 

%  Initialize  the  limit  of  the  Disturbance  generator  voltage.  This 
%  protects  the  bass  shaker  from  exposure  to  excessive  voltage  levels  as 
%  amplitudes  are  sometimes  varying  with  time,  and  if  not  monitored 
%  closely  could  get  too  high.  Limit  expressed  in  +/-  volts  at  the 
%  amplifier  input. 

Dist_limit  =4.0; 

%  Initialize  the  sizes  of  the  disturbance  related  parameter  vectors. 

%  This  way,  the  set_dist.m  file  can  be  used  while  the  DSP  is  running  to 
%  change  the  number  of  disturbances  frequencies  that  are  present  (a 
%  maximum  of  5  unless  these  vectors  are  made  bigger,  and  the 
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%  build/download  process  is  repeated) . 


% 

%  dist_ampO 
%  amp_rate 
%  dist_freqO 
%  freq_rate 


=  initial  amplitude  (volts  at  Kepco  amplifier  input) 

=  rate  of  amplitude  change  (volts/sec) 

=  initial  frequencies  conv.  to  rad/sec 

=  rate  of  frequency  change  (Hz/sec)  conv.  to  rad/sec/sec 


%  phase 

=  Initial  phase 

angle 

of  sinusoid 

conv. 

to  radians 

dist_amp0  = [ 

0.0  0.0 

0.0 

0.0 

0.0 

]  ; 

amp_rate  = [ 

0.0  0.0 

0.0 

0.0 

0.0 

]  ; 

dist_freqO= [ 

0.0  0.0 

0.0 

0.0 

0.0 

] *2*pi; 

f req_rate  = [ 

0.0  0.0 

0.0 

0.0 

0.0 

] *2  *pi ; 

phase  = [ 

0.0  0.0 

0.0 

0.0 

0.0 

] *pi/180 ; 

%  Initialize 

the  sample  rate  of 

the 

controller  and  I/O 

functions 

sample_rate  =1000;  %  Controller  sample  rate  (Hz) 

dt=l/sample_rate;  %  Sample  period 


io_rate=10*sample_rate;  %  Sample  rate  of  A/D  &  D/A 

dt_io=l/io„rate; 
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HOST  PC  SUPERVISORY  CODE 


%  start__uqp.m  Stephen  Edwards  10  May  99 

% 

%  This  script  file  is  the  Matlab  code  that  oversees  execution  of  the 
%  control  experiments  on  the  Ultra  Quiet  Platform  using  the  clear  box 
%  method. 

% 

%  To  use  this  file  you  must: 

%  1)  At  the  matlab  prompt  type  "uqp"  (in  directory  c:\sge\cbox) 

%  -  this  opens  the  Simulink  diagram  that  represents  the  DSP 

%  code 

%  -  it  also  automatically  initializes  the  variables  defined 

%  in  the  file  "init_uqp.m" 

%  2)  "Download"  the  DSP  code  from  the  Simulink  model  "uqp.mdl" 

%  using  the  Multiprocessor  Setup  window  in  the  Simulink  Diagram 

%  (not  RTW  build!) 

%  3)  At  the  Matlab  prompt  type  "start_uqp" ,  and  follow  the  prompts 


%  First  initialize  the  MLIB  and  MTRACE  environments.  This  gets  the 
%  trace  file  variable  descriptions  (addresses)  for  the  various 
%  parameters  used  in  the  DSP  code  (flags,  disturbance  levels,  etc) . 

%  Once  this  routine  is  run,  the  various  Matlab  prompt  commands  can  be 
%  used  (i.e.  "noise.on",  "control_of f " ,  etc.) 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Prompt  for  ID/Control  Selections  and  Initialize  DSP  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

fprintf ( ' \n\n\n  WELCOME  TO  THE  UQP  CONTROLLER  -  GOOD  LUCK!  \n\n') 

da t a_dur a t i on  =  600;  %  Duration  of  exp  data 

experiment_duration  =  data_duration  +1;  %  Duration  in  seconds 

%  Ask  the  user  what  type  of  experiment  to  perform 
exp__type  =input  ( [ '  \n 1  ,  .  .  . 

' \n  What  Type  of  Experiment ? \n 1 . 

* \n  1  =  Perform  system  ID  before  initiating  controller',... 

' \n  2  =  Initiate  controller  using  the  stored  reference  model',... 
' \n  3  =  Initiate  controller  using  model  from  last  experiment',... 
' \n  Choice  =  ' ] ) ; 

control_type  =input ( [ ' \n ' , . . . 

' \n  What  Type  of  Controller  are  you  set  up  for?\n',... 

'  \n  1  =  Sine  /  Cosine  Method' , . . . 

* \n  2  =  Eta  Method' , . . . 

' \n  Choice  =  ' ]  )  ; 
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switch  control_type 

case  1  %  sine/cosine-controller 

f req_id__meth  =input ( [ ' \n ’ , .  .  . 

' \n  What  Type  of  Frequency  ID?  \n' , . . . 

' \n  1  =  Perform  disturbance  freq  ID  using  input/output  data',... 
' \n  2  =  Perform  disturbance  freq  ID  using  disturb,  effect1,... 
'data  [recommended] ' , . . . 

1 \n  3  =  Deliberately  assign  incorrect  frequency (ies) . 

1 \n  Choice  =  ' ] ) ; 


switch  freq_id__meth 
case  1 

dist_loop_duration  =  10; 


%  seconds  (choose  larger  value 
%  since  the  controller  must  be 
%  turned  off  to  identify  the  dist. 

%  frequencies,  so  you  don't  want  to 
%  do  it  as  often) 


case  2 

dist_loop_duration 


1.0;  %  seconds  (choose  smaller  value 

%  since  the  controller  can  stay  on 
%  while  identifying  the  disturbance 
%  frequencies) 


case  3 

dist_loop_duration  =  experiment_duration;  %  Prevents  looping 
f req_error  =input ( [ ' \n ' , . . . 

' \n  Enter  the  percent  error  to  use  for  the  disturbance',... 


'  frequency: ' , . . . 

'  \n  [i.e.  "1"  equals  1  percent,  negative  values  OK]',... 
■  \n  Choice  =  '  ]  )  ; 


end 

case  2  %  eta-controller 

%  Set  variables  used  in  eta  controller 

N= input (*\n  What  is  Value  of  N  you  are  using?  :  '); 

basis_ set_code=l ;  %  set  #1  =  {2,  7,  9,  12,  16,  22,  23,  30} 

%  this  code  stored  in  archive  data  file, 
f lag_eta_f ilter  =input ( [ ' \n* , . . . 

' \n  Do  you  want  to  filter  the  eta  signal?\n' , . . . 

' \n  1  =  yes ' , . . . 

' \n  0  =  no 1 , . . . 

' \n  Choice  =  1 ] ) ; 
switch  flag_eta_f ilter 
case  0 

%  do  nothing  -  filter  not  used  in  eta  controller  S-Function 
case  1 

[ f ilt_order , f ilt_alpha, f ilt_beta] =f ilter_design_butter (dt) ; 
%  This  is  a  simple  function  to  design  a  Butterworth  filter 
%  using  the  Matlab  Signal  Processing  Toolbox  commands 
%  (not  provided  here) 

end 

end 
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%  Initialize  the  environment  for  dSPACE  MLIB  and  MTRACE  commands 
init_mlib 

%  This  is  a  Matlab  script  file  to  assign  variable  names  to  the 
%  addresses  of  required  DSP  variables,  allowing  the  passing  of 
%  variables  to  and  from  the  DSP 

%  Make  sure  configuration  is  initialized  (simple  Matlab  script  file 

%  files  not  included  here) . 

reset_uqp 

control_of f 

update_of f 

noise_of f 

%  Load  filter  if  appropriate 
if  f lag_eta__f ilter  ==  1 
.  load_filter 
end 

%  Select  RLS  parameters  best  suited  to  control  method 

%  Future  option  -  can  use  set_lambda  (forgetting  factor)  and  set_p_init 
%  -  (covariance)  to  modify  variable  in  DSP 

%  Set  up  the  disturbances 

fprintf ( ' \n\n\n  DISTURBANCE  SET-UP  FOR  THIS  EXPERIMENT:  \n\n') 
set_dist 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Perform  System  Identification,  if  Selected  Above  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

switch  exp„type 

case  1  %%%  case  =  Do  ID  First,  then  Control  %%% 

fprintf ( 1  \n  »>  Starting  System  ID  \n') 

%  Initialize  and  turn  on  the  excitation  noise  (all  6  struts  at  once) 
id_type= '  system  %  field  must  have  the  same  number  of  spaces 

%  as  the  word  "disturbance" 

dist_on 

set_noise_auto  %  Set  levels  of  white  noise 

pause(l);  %  Let  initial  turn-on  transients  die  out 

%  Get  the  input  &  output  data  and  store  in  the  variable  name 
%  ' trace_data ' 

num_sec=10 . 0 ;  %  The  duration  of  the  data  capture 

get_sys_id_data  %  A  Matlab  script  file  (not  provided) 

noise_off  %  Turn  off  the  6-strut  excitation 

%  Use  a  batch  version  of  the  Clear  Box  method  to  get  the  system 
%  disturbance- free  model,  the  disturbance  frequency (ies)  and  damping 
%  ratio(s),  and  the  chosen  model  order,  p. 

[alpha_bar,beta_bar,theta_bar,d_freqs,d_damps,p]=. . . 

cbox__sys_id(trace„data,p,dt)  ; 
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%  Download  Disturbance-Free  Model  to  DSP 
load__model 

%  Note:  Model  data  is  saved  during  cbox_batch  function  to: 

%  c : \sge\cbox\models\current_cbox„model 

case  2  %  Start  controller  using  reference  model  loaded  earlier  % 

id_type= ' none  ' ;  %  field  must  have  the  same  number  of  spaces 

%  as  the  word  "disturbance" 


switch  p 
case  5 

load  models\cbox __p05_10sec 
case  10 

load  models\cbox__pl0_10sec 
case  15 

load  models\cbox_pl5_10sec 
case  20 

load  models\cbox_p20_10sec 
case  25 

load  models \cbox_p25_10sec 
case  30 

load  models\cbox_j?30_10sec 
case  40 

load  models\cbox__p40__10sec 

end 


theta_bar 

alpha_bar 

beta_bar 

theta_bar 

alpha_bar 

beta_bar 

theta_bar 

alpha_bar 

beta_bar 

theta_bar 

alpha_bar 

beta_bar 

theta_bar 

alpha_bar 

beta_bar 

theta_bar 

alpha_bar 

beta_bar 

theta__bar 

alpha_bar 

beta_bar 

load_model  %  Download  Disturbance-Free  Model  to  the  DSP 

case  3  %  Start  controller  using  model  identified  in  last  experiment  % 

id_type= 'none  %  field  must  have  the  same  number  of  spaces 

%  as  the  word  "disturbance" 

load  models\current_cbox_model  theta_bar  beta_bar 
%  Download  Disturbance-Free  Model  (loaded  in  init_uqp.m  or  done  in 
%  prior  experiment)  to  the  DSP 
load_model 

end  %  switch  for  experiment  type 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Start  Selected  Controller  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


start_time=clock; 
first__time  =  1; 

dist_on 


%  Mark  this  time  as  the  start  of  the  experiment 
%  Used  to  control  output  to  screen  using  eta 
%  freq  ID  method 

%  Turn  on  the  disturbance,  if  not  already  on 
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while  (etime (clock, start_time)  <  experiment_duration) 
beginJLoop=clock; 
switch  control_type 

case  1  %  Controller  using  sine/cosine  basis 

switch  freq_id_meth 

case  1  %  Sine/cosine  -  Disturbance  freq  ID  via 

%  input /output  data 

id_type= 1  disturbance ' ; 

update__off  %  Disable  control  coefficient  updating 

control_off  %  Turn  off  control  signal 

set_noise_auto  %  Set  white  noise  levels 

paused);  %  Let  initial  turn-on  transients  die  out 

num_sec=3 . 0;  %  Duration  of  data  capture 

get_in_out__data  %  Get  the  input  &  output  data  and  store 

%  in  the  variable  name  ' trace_data ' 
noise_off  %  Turn  off  the  6-strut  excitation 

%  Do  disturbance  ID 
p_dist=10 ; 

[d_f  reqs ,  d_damps  ]  =  .  .  . 

cbox_freq_id  ( trace__data,  p_dist ,  dt__data,  max_.fr  eq)  ; 

%  Note:  the  above  function  is  essentially  the  same  as 
%  "cbox_sys_id" ,  except  the  routine  stops  as  soon  as  the 
%  frequencies  are  identified,  thus  saving  computational 
%  resources,  (thus  this  routine  is  not  provided) 

%  Determine  disturbance  info  from  cbox_freq_id  output 
nfreq  =length (d_freqs) ; 

control_f reqs  =zeros ( 1 , max_f req) ; 

control_f reqs ( 1 : nfreq)  =d_f reqs ; 

load_freqs  %  Download  frequencies  to  DSP 

%  If  first  time  through  start  data  acquisition 
if  f irst_time  -=  1 

dist_reset  %  If  disturbances  are  using  a  variation 

%  profile,  this  will  reset  them  back  to 
%  their  initial  conditions. 
dist_on  %  Turn  on  the  disturbance 

num_sec=data_duration; 

dt_exper_data=ceil (num_sec/3) *dt;  %  Prevents  overflow  of 

%  memory  buffer. 

get_exper_data  %  Capture  data 

end 

control_on  %  Turn  on  controller 

update_on 
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%  Pause  until  time  for  the  next  frequency  update 
% (dist_loop_duration  set  at  beginning  of  "start_uqp .m"  ) 
while (etime ( clock, begin_loop)  <  dist_loop_duration) ,  end 
first_time  =  0; 

case  2  %  Sine/Cosine  Method  - 

%  Disturbance  freq  ID  via  disturbance  effect  data 


num_sec=dist_loop_duration-0 . 5; 

dt__eta_data=ceil (num_sec/3) *dt;  %  Prevents  memory  overflow 
get_eta__data  %  Get  the  disturbance  effect 

%  data  and  store  in  the 
%  variable  name  ' eta— hist' 

%  Do  disturbance  frequency  ID  (AR  model  order  =  tau) 
tau=10 ; 

[d_f  reqs ,  d_damps  ]  = .  .  . 

eta2freq__id  (eta_hist ,  tau,  dt_eta__data,max_freq,  f  irst__time)  ; 

%  Determine  disturbance  info  from  eta2freq_id  output 
nf req= length (d_f reqs) ; 
control_freqs=zeros (l,max_freq) ; 
control_f reqs ( 1 : nf req) =d_f reqs ; 

load_freqs  %  Download  frequencies  to  DSP 

%  If  first  time  through  start  data  acquisition,  and  turn  on 
%  controller /update 
if  first_time  ==  1 

dist_reset_no_echo  %  These  versions  prevent 

dist_on_no_echo  %  output  to  the  screen 

num_sec=data_duration; 

dt_exper_data=ceil (num_sec/3) *dt;  %  Prevents  overflow  of 

%  memory  buffer 

get_exper_data 

control__on_no_echo 

update_on_no__echo 

end 

%  Pause  until  time  for  the  next  frequency  update 

while (etime (clock, begin_loop)  <  dist_loop_duration) ,  end 

first_time  =  0; 

case  3  %  Sine/Cosine  Method  - 

%  Deliberately  load  the  incorrect  frequency 

d__freqs  =dist_freqO  (1  :num_dist )  *  (1  +  freq_error/100)  ; 

nfreq  =length (d_freqs) ; 

control_f reqs  =zeros ( 1 , max_f req) ; 
control_f reqs ( 1 : nfreq)  =d_f reqs ; 

load_freqs  %  Download  frequencies  to  DSP 
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%  If  first  time  through  start  data  acquisition 
if  f irst_time  ==  1 

num_sec=data_duration; 

dt_exper_data=ceil (num_sec/3) *dt;  %  Prevents  overflow  of 

%  memory  buffer 

get_exper_data 

end 

%  Turn  on  controller 

control_on 

update_on 

%  Pause  until  time  for  the  next  frequency  update 

while (etime (clock, begin_loop)  <  dist_loop_duration) ,  end 

first_time  =  0; 


end 

case  2  %  Adaptive  Basis  Method 

num_sec=data__duration; 

dt_exper_data=ceil (num_sec/3) *dt;  %  Prevents  overflow  of 

%  memory  buffer 

get_exper__data 

control_on 

update_on 

while (etime (clock, begin_loop)  <  experiment_duration) ,  end 

%  Do  nothing  -  S-Function  uses  eta  time  history  as  the  basis  for 
%  the  control  signal,  so  the  frequencies  do  not  need  to  be 
%  estimated. 


end 


end  %  Experiment  Complete 

%  Get  and  save  the  experiment  data  (script  file  not  provided) 
fetch_exper_data 

%  Turn  off  disturbance,  noise  (if  present) ,  controller,  and  updates 
stop 

%  Let  transients  die  out,  then  take  noise  level  readings 
pause (2) ; 
num__sec=3 . 0; 

dt_noise_data=ceil  (num__sec/3)  *dt; 

get_noise_data  %  saved  in  variable  'noise__data' 

save  models \current_noise_data  noise_data  dt_noise_data 

plot_experiment_results  %  Plot  results 

fprintf ( 1 \n\n\n  STOP  TIME  REACHED  -  EXPERIMENT  COMPLETE  \n\n‘) 
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SYSTEM  IDENTIFICATION 


function  [alpha_bar,beta_bar,  theta_bar,  cLfreqs,  d_damps,p]  = .  .  . 

cbox_sys__id  ( trace_data , p , dt ) 


%  Do  system  id  based  on  "noisy"  input  /  output  data  using  the 
%  "clear  box"  method  of  Phan  &  Goodzeit. 


%  alpha,  beta: 
%  theta: 

%  p: 

%  f : 

%  u,  y,  dt: 

%  Ap,  Bp,  Cp: 

%  T: 

%  lambda : 

%  Lambda : 

%  Gamma : 

%  Omega : 


ARX  model  coefficients 

[alpha  beta]  coefficients  combined  into  one  matrix 
order  of  ARX  model 

number  of  disturbance  frequencies  to  eliminate 
Input  /  output  data,  sample  time 
State  space  model  in  observer  canonical  form 
Eigenvector  Matrix  of  Ap 

Eigenvalue  matrix  (block  diagonals  =  eigenvalues) 
Ap  transformed  to  modal  form 
Bp  transformed  to  modal  form 
Cp  transformed  to  modal  form 


%  Determine  DARMA  &  State  Space  Models  from  the  data,  diagonalize,  and 
%  find  the  disturbance  frequency (ies) .  The  wn  vector  is  sorted  by 
%  lowest  to  highest  damping  ratio,  so  the  first  wn  is  the  most  likely 
%  to  be  a  disturbance. 


%  Calculate  the  DARMA  model  coefficients  associated  with  the  input/ 

%  output  data  supplied: 

% 

%  y (k)  =  alpha (1) *y(k-l)  +  alpha  (2)  *y  (k-2)  +  ...  +  alpha (p) *y (k-p) 

%  +  beta(l) *u(k-l)  +  beta(2) *u(k-2)  +  ...  +  beta (p) *u (k-p) 

% 


%  Assume  number  of  inputs  and  outputs  is  6  each  (Full  MIMO  for  UQP) . 
%  Routine  is  coded  for  general  MIMO  with  m  inputs  and  q  outputs. 


in_struts  =[12345  6]; 
out__struts  =[1  2  3  4  5  6]; 

m=length(  in_struts) ; 
q=length(out_struts) ; 

u=trace_data (in_struts, : ) ; 
y=trace_data (out_struts+6 , : ) ; 

L=length(u) -1; 


%  Number  of  Inputs 
%  Number  of  Outputs 

%  Each  row  is  time  history  of  inputs 
%  Each  row  is  time  history  of  outputs 

%  number  of  data  points  in 
%  input/output  data  (k=0 ,1,2, . . L) 


n„imp=100 ; 


%  The  number  of  impulse  response  data 
%  points  to  calculate 
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%  Set  the  damping  threshold  (below  which  a  mode  is  considered  a 
%  disturbance)  -  the  threshold  should  usually  be  set  lower  for  higher 
%  model  order. 

switch  p 
case  5 

damp_thresh=le-l ; 
case  10 

damp_thresh=le-l ; 
case  15 

damp_thresh=5e-3 ; 
case  20 

damp_thresh=5e-3 ; 
case  25 

damp_t hr e  sh= 1 e - 4 ; 
case  30 

damp_thresh=le-4; 
case  40 

damp_thresh=le-4 ; 
otherwise 

damp__thresh=5e-2  ; 

end 

%  Build  Y  Sc  V  (See  Goodzeit  &  Phan  MAE  Tech  Report  #2096  - 
%  Princeton  University) 

fprintf ( ' \n\n  »>  Organizing  input/output  data  \n') 

Y=y ( : , p+1 : L+l) ; 

V=zeros (p*q+p*m, L-p+1) ; 
for  j=l:p 

V(  ( j  — 1 ) *m+l  :  j  *m,  : )  =  u ( : , j :L-p+j ) ; 

V(p*m  +  (j-l)*q+l  :  p*m+j*q,:)  =  y ( : , j : L-p+ j ) ; 
end 

%  Use  the  pseudoinverse  to  determine  the  ARX  model  coefficients,  alpha 
%  and  beta 

fprintf ( ' \n  »>  Calculating  ARX  model  coefficients  \n ' ) 

K  =Y*V'  *pinv(V*V’  )  ; 

CC_MT  =K ( : ,  l:p*m); 

Minus_CM  =K( : ,p*m+l:p*m+p*q) ; 

clear  K  Y  V  %  save  memory  space 

alpha  = [] ; 
beta  = [ ] ; 
for  j  =1 :p 

alpha  = [alpha  Minus_CM ( : , p*q- j *q+l :p*q- ( j -1) *q) ] ; 
beta  = [beta  CC_MT( : ,p*m-j*m+l :p*m- ( j -1 ) *m) ] ; 

end 

clear  Minus_CM  CC__MT 
theta= [alpha' ;beta' ] ; 

fprintf  (  '  \n  »>  ARX  model  complete  \n  ') 
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%  Transform  to  State  Space  Observable  Canonical  form 
[ Ap , Bp , Cp , Dp ] =arx2ss (theta,p) ; 

fprintf (*\n  »>  Converted  to  State  Space  form  \n  ') 

%  Transform  to  Modal  (Block  Diagonal)  Form  and  calculate 
%  wn  &  zeta  for  all  modes 

[Am,Bm,Cm,Dm,num_modes,wn,  zeta,m_idx]  =  ss2modal  (Ap,  Bp,  Cp,  Dp,  dt)  ; 

%  Determine  which  modes  are  disturbance  modes  (by  index) ,  and  how  much 
%  each  mode  impacts  the  total  pulse  response  of  the  system 
[dist_modes,mode_norms]  = .  .  . 

analyze__modes  (Am,Bm,Cm,Dm,num_modes,wn,  zeta,m_idx,  .  .  . 
damp_thresh,  n_imp ,  dt )  ; 

%  Output  frequency  information  to  the  screen 

fprintf ('\n  »>  Frequencies  identified  as  disturbances  are:  \n') 
for  i=l: length (dist_modes) 

fprintf ( 1 %10d  -  %9.4f  Hz  \n ' , i, wn (dist_modes (i) ) /2/pi) 

end 

%  Eliminate  the  disturbance  modes  from  the  state  space  model 
[  A_bar ,  B_bar ,  C_bar ,  D_bar  ]  =e  1  im_di  s  t  ( Am ,  Bm ,  Cm ,  Dm ,  wn ,  di  s  t_mode s ,  m_i dx )  ; 
fprintf ( ' \n  »>  Disturbances  Eliminated  from  State  Space  Model  \n  ') 

%  Return  model  to  ARX  form  (keep  its  order  =  p) 

fprintf ( ‘ \n  »>  Computing  disturbance- free  ARX  model  \n  ') 

[alpha_bar , beta_bar]  =ss2arx (A__bar , B_bar , C_bar ,p)  ; 

theta_bar= [alpha_bar ' ;beta_bar ' ] ; 

%  Save  the  model  and  data 

save  model s\current_cbox_model  alpha_bar  beta_bar  theta_bar  p  dt  A_bar 
B_bar  C_bar  D_bar  trace_data 

save  models\current_model_data  trace_data  dt 

save  models\thesis_dist_corrupt_model  Ap  Bp  Cp  Dp  dt 

d_freqs=wn(dist_modes) ; 
d_damps=zeta (dist_modes) ; 

plot_damp_and_norm  %  Plots  damping  ratio  and  pulse  response 

%  norms  for  each  mode  of  the  system 
%  (not  provided) 


180 


FREQUENCY  IDENTIFICATION  FROM  DISTURBANCE  EFFECT  DATA 


function  [cLfreqs , d_damps] = . . . 

eta2freq_id(eta_hist ,p,  dt  ,max__freq,  f  irst_time) 


%  Stephen  Edwards 


May  1999 


%  Identify  an  AR  model  for  the  disturbance  effect  data 


%  p: 

%  f : 

%  y,  dt: 

%  max_freq 
% 


order  of  ARX  model 

number  of  disturbance  frequencies  to  eliminate 
eta  data,  sample  time 

The  maximum  number  of  disturbance  frequencies  that 
can  be  controlled 


%  Assume  number  of  outputs  is 
%  coded  for  general  MIMO  with 

out_struts  =[1  23456]; 
q=length(out_struts) ; 
y=eta_hist (out_struts, : ) ; 
L=length (y) -1; 


damp_thresh=le-3 ; 


if  first_time  ==  1 

fprintf(‘\n  »>  Estimating 
fprintf('\n  »>  Identified 

end 

%  Build  Y  &  V 

%  (See  Goodzeit  &  Phan  MAE  Tech  Report  #2096  -  Princeton  University) 
Y=y( : ,p+l:L+l) ; 

V=zeros (p*q,L-p+l) ; 
for  j=l:p 

rows  =  [(j-l)*q+l  :  j*q]; 
cols  =  [ j :L-p+ j ] ; 

V (  rows  ,  :  )  =  y(  :  ,  cols  ); 
end 

clear  rows  cols 

%  Use  the  pseudoinverse  to  determine  the  AR  model  coefficients,  gamma 
K  =Y*V '  *pinv  (V*V'  )  ; 
gamma  = [ ] ; 


6  (Full  MIMO  for  UQP) .  The  routine  is 
m  inputs  and  q  outputs. 


%  Number  of  Outputs 

%  Each  row  is  time  histoy  of  outputs 
%  number  of  data  points  in  eta  data 
%  (k=0 ,1,2, . .L) 

%  Set  the  damping  threshold  (below 
%  which  a  mode  is  considered  a 
%  disturbance) 


disturbance  frequencies....  \n') 
disturbance  Frequencies  are  [Hz]:  \n‘) 
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for  j  =1 :p 

gamma  = [gamma  K( : ,p*q- j *q+l :p*q- ( j -1) *q) ] ; 

end 

clear  K  Y  V 

%  Transform  to  State  Space  Observable  Canonical  form 
[Am,num_modes,wn, zeta,m_idx]  =  ar2modal (gamma ,p, dt) ; 

%  Determine  which  modes  are  disturbance  modes  (by  index) 
[dist_modes,not_controlled]=.  .  . 

analyze_eta_model  (wn,  zeta,  damp_thresh,max_freq)  ; 

%  Print  out  info  to  the  screen 
if  is  empty ( di s  t_mode s ) 
fprintf ( 1 \n  None') 
else 

fprintf ('\n  ■) 

for  i=l : length (dist_modes) 

fprintf ( '  %11 . 4f ' ,  wn  (dist__modes (i) ) /2/pi) 

end 

if  ~isempty (not_controlled) 

fprintf ('  ***  Dist  Freqs  Not  Controlled  =  ') 

for  i=l : length (not_controlled) 

fprintf ( '  %11.4f ' , wn (not_controlled(i) ) /2/pi) 

end 

end 

end 

d_freqs=wn(dist_modes) ; 
d_damps=zeta  (dist__modes)  ; 

%  Plot  mode  damping  ratios 
figure (2 ) 

semilogy (wn/2/pi,abs (zeta) , 'bx' , . . . 

wn (dist_modes) /2/pi, abs (zeta (dist_modes) ) , 'ro' , . . . 

[0  500]  ,  [damp__thresh  damp_thresh]  ,  'r —  * ) 
title ( [ 'Disturbance  ID  Mode:  Damping  Ratio  vs.  Frequency,  tau  =  ' , . . . 
num2str (p) ] ) 

text (300 , damp_thresh*l . 5 ,' Damping  Ratio  Threshold') 
axis([0  500  le-7  max (abs ( zeta) )] ) 
grid  on 
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CONVERSION  FROM  ARX  TO  STATE  SPACE  FORM 


function  [A, B,C,D] =arx2ss ( theta, p) 

%  File  provided  courtesy  of  Neil  Goodzeit,  Lockheed  Martin  Missiles  & 
%  Space  (©  Copyright  by  Neil  E.  Goodzeit,  1998,  All  Rights  Reserved) 

%  ....  Calculate  the  number  of  inputs  and  outputs 
n__out  =  min  (size  (theta)  )  ; 
n_in  =  (length (theta)  -  p*n_out)  /  p  ; 

%  ....  The  ARX  model  response  coefficients 
idx  =  p*n_out  ; 
alpha  =  theta (1 : idx, 1 :n_out) '  ; 

%  ....  The  ARX  model  input  coefficients 

beta  =  theta (idx+1 : idx+p*n_in, 1 :n_out) 1  ; 

%  ....  Construct  the  state  space  model  in  observable  canonical  form 
A  =  [  ]  ; 

B  =  [  ]  ; 

for  j  =  l:p 


jl  =  j-i  ; 

idx  =  jl  *  n_out  +  1  ; 


if  j<p 

A  =  [  A  ;  [  alpha (:, idx: idx+n_out-l) ,  ... 

zeros (n_out, jl*n__ out) ,  eye (n_out , n_out ) ,  . . . 

zeros  (n__out,  (p-j-1)  *n_out)  ]  ]  ; 

else 

A  =  [  A  ;  [  alpha ( : , idx: idx+n_out-l) ,  ... 

zeros (n_out , j l*n_out)  ]  ]  ; 

end 


B  =  [  B  ;  beta (:, idx: idx+n_out-l)  ]  ; 
end 


C  =  [  eye  (n_out  ,n_out)  ,  zeros  (n_out  ,n__out*  (p-1)  )  ]  ; 
D  =  zeros (n_out,n_in)  ; 
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DIAGONALIZATION  OF  STATE  SPACE  MODEL 


f  unc  t  i  on  [  Am ,  Bm ,  Cm ,  Dm ,  n_mode ,  w_mode ,  z_mode ,  m_i  dx  ]  = 

ss2modal  (Ap,  Bp,  Cp,  Dp,  dt ) 

%  ss2modal.m 

% - - 

%  ...  File  provided  courtesy  of  Neil  Goodzeit,  Lockheed  Martin  Missiles 
%  Sc  Space  (©  Copyright  by  Neil  E.  Goodzeit,  1998,  All  Rights 

%  Reserved) 

% 

%  ...  Modified  by  Stephen  Edwards  4  May  99 
% 

%  ...  Convert  the  input  state-space  model  to  modal  form 
% 

%  ...  Input  parameters : 

% 

%  ...  Ap,  Bp,  Cp,  Dp  the  input  state-space  model 

%  . . .  dt  the  sampling  interval 

% 

%  ...  Output  parameters : 

% 

%  ...  Am,  Bm,  Cm,  Dm  the  state-space  model  matrices  in  modal  form 
%  ...  n_mode  the  number  of  modes 

%  ...  w_mode  the  mode  frequency  (rad/ sec) 

%  ...  z_mode  the  mode  damping  ratio 

%  ...  m_idx  the  mode  index  matrix  (n_mode,2) 

% 

% - 

%  ...  Transform  the  model  to  modal  coordinates 
[V,  Am]  =  eig(Ap); 

i_eig  =  1; 
n_mode  =  0 ; 

for  j  =  1: length (V)  %  Iterate  on  each  eigenvector  column 

if  imag (V ( 1 , j ) )  ==  0  %  Real  Eigenvector  (1st  order  mode) 

n_mode  =  n__mode  +  1; 

s  =  log(Am( j , j ) ) /dt;  %  See  annotations  below 

w_mode ( n_mode )  =  abs ( s )  ; 

z_mode (n_mode)  =  -real ( s ) /w_mode (n_mode ) ; 
m_idx  (n_mode,  1:2)  =[  j  ,  j  ]; 


%  continued  on  next  page 
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%  Complex  Eigenvector  (2nd  order  mode) 
%  1st  column  of  complex  conjugate 


else 

if  i_eig  == 


V ( : , j )  =  real (V( : ,  j ) )  ; 
i_eig  =  2 ; 
n_mode  =  n_mode  +  1; 
s  =  log (Am (j , j ) ) /dt; 

w_mode  ( n_mode )  =  abs  ( s )  ; 

z_mode (n_mode)  =  -real (s) 

m_idx  (n__mode,  1:2)  =  [  j 

elseif  i_eig  ==  2 


%  Take  real  part 


%  z=eA(s*T)  — >  s=(ln(z))/T 
%  s=-sigma+j *wd 
%  wn=sqrt (sigmaA2+wdA2) 

w_mode (n_mode) ; 

%  zeta=sigma/wn 

,  j+1  ] ; 

%  2nd  column  of  complex  conj . 


V  ( :  ,  j  )  =  imag ( V ( : , j ) ) ;  %  Take  imaginary  part 

i_eig  =  1; 


end 


end 


end 

%  ...  Calculate  the  poles  and  the  modal  coordinate  state-space  matrices 

Am  =  inv(V)*Ap*V  ; 

Bm  =  inv(V)*Bp  ; 

Cm  =  Cp*V 

Dm  =  zeros (size (Cm, 1) , size (Bm, 2 ) )  ; 
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CONVERSION  FROM  STATE  SPACE  TO  ARX  FORM 


function  [alpha, beta] =ss2arx(A, B,C,p) 

%  ss2arx.m  Stephen  Edwards  May  1999 

%  Some  elements  of  the  code  provided  by  Neil  Goodzeit,  Lockheed 
%  Martin  Missiles  &  Space 

%  (©  Copyright  by  Neil  E.  Goodzeit,  1998,  All  Rights  Reserved) 

%  Calculate  the  order  p  ARX  model  coefficients 
%  from  the  state-space  model  matrices  A,  B,  C 

%  alpha  is  the  coefficient  matrix  of  the  past  response 
%  beta  is  the  coefficient  matrix  of  the  past  input 

n_in  =  min ( size (B) )  ; 
n_out  =  min ( size (C) )  ; 
n  =  length (A)  ; 

%  Initialize  matrices 
Ap  =  eye (size (A))  ; 

Op  =  C  ; 

Cp  =  B  ; 

Tp  =  [  zeros (n_out,n_in)  ;  C*B  ]  ; 

%  Raise  A  to  the  p  power 
for  i  =  l:p 

if  i  ~=  1 

%  The  observability  and  controlability  matrices 
Op  =  [Op  ;  C*Ap  ]  ; 

Cp  =  [  Ap*B,  Cp  ]  ; 

%  The  Toeplitz  Matrix 
if  i~=  p 

Tp  =  [  Tp  ;  C*Ap*B  ]  ; 

end 


end 

Ap  =  Ap  *  A  ; 
end 

%  Construct  the  toeplitz  matrix  from  the  first  column 
for  i  =  2:p 

%  The  row  and  column  indices 
col  =  (i-2)*n_in  +  1  ; 
row  =  (i-l)*n_out  +  1  ; 


186 


n_rows 


(p  -  i)*n_out  ; 


if  i  ==  p 

Tp  =  [  Tp,  zeros (p*n_out ,  n_in)  ]  ; 
else 

Tp  =  [  Tp,  [zeros (i*n_out , n_in)  ;  Tp(row:row+n_rows- 

l,col:col+n_in-l)  ]  ]  ; 

end 

end 

%  Calculate  the  observer  gain  matrix 
M  =  -Ap*pinv (Op)  ; 

%  Calculate  the  ARX  model  coefficients  (  ordered  from  k-p  to  k-1) 
%  a's  are  coefficients  of  y,  b's  are  coefficients  of  u 
a  =  -C*M  ; 
b  =  C*(Cp  +  M*Tp)  ; 

alpha  =  []  ; 
beta  =  [  ]  ; 

%  Reorder  the  coefficients 
for  i  =  1 sp 

id_a  =  (p-i) *n_out+l  ; 
id_b  =  (p-i)*n_in+l  ; 

alpha  =  [alpha,  a ( : , id_a : id_a+n_out-l)  ]  ; 
beta  =  [beta,  b( : , id_b: id_b+n_in-l)  ]  ; 

end 
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ELIMINATION  OF  DISTURBANCE  MODES 

function 

[A_bar, B_bar , C„bar , D_bar] =elim_dist (Am,  Bm, Cm, Dm,  wn,  dist_modes  ,m_J.dx) 

%  elim_dist.m  Stephen  Edwards  5  May  99 

% 

%  A  function  to  eliminate  the  disturbance  modes  from  the  modal  state 
%  space  model . 

% 

%  Input  Parameters : 


% 

%  Am, Bm, Cm, Dm  .  State  Space  model  in  modal  form 

%  wn  .  Natural  frequencies  of  the  modes 

%  dist_modes  . A  vector  of  the  indeces  for  the  disturbance  modes 

%  (each  2nd  order  mode  has  one  index  value) 

%  m_idx  .  Identified  the  states  associated  with  each  mode 

%  (one  row  for  each  mode) 

% 


%  Output  Parameters : 
% 

%  A_bar , B_bar , 


%  C_bar,D_bar  .  Disturbance-free  State  Space  model 

%  p_bar  . p  value  of  disturbance  free  model 


A_bar-Am; 

B_bar=Bm; 

C_bar=Cm; 

D_bar=Dm; 

num__dist=length(dist_modes)  ; 
states_to_elim= [  ]  ; 

for  i=l:num_dist 

%  Identify  disturbance  mode  rows  and  columns 

states_to_elim= [states_to„elim,  m_idx (dist_modes (i) , : ) ] ; 

fprintf ( ' \n  »>  %9.4f  Hz  mode  eliminated  \n ' , wn (dist_modes ( i ) ) /2/pi) 

end 

A__bar  (states_to_elim,  : )  =  [  ]  ;  %  Remove  rows 

B_bar (states_to_elim,  :)  =  []; 

A__bar  ( : ,  states_to_elim)  =  [  ]  ;  %  Remove  columns 

C_bar ( : , states_to_elim) = [ ] ; 
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SYSTEM  MODE  ANALYSIS 


function  [dist„modes,mode_norms]  =  .  .  . 

analyze_modes  (Am,  Bm,  Cm,  Dm,num_modes ,  wn,  zeta,m_idx,  damp_thresh, n_imp,  dt ) 


%  analyze_modes .m 


% - -- 

%  ...  Original  file  provided  courtesy  of  Neil  Goodzeit,  Lockheed  Martin 
%  Missiles  &  Space  (©  Copyright  by  Neil  E.  Goodzeit,  1998, 

%  All  Rights  Reserved) 

% 

%  ...  Modified  by  Stephen  Edwards,  May  99 


% 


% 

% 

% 

% 

% 


% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 


.  . .  Input  parameters : 


.  .  .  Am,  Bm,  Cm,  Dm 
.  .  .  num_modes 
.  . .  wn,  zeta 
.  . .  m_idx 

.  . .  damp_ thresh 

.  .  .  n_imp 
.  .  .  dt 


the  state-space  model  matrices  in  modal  form 
the  number  of  modes 

the  modal  frequencies  (rad/ sec) ,  damping  ratios 

the  mode  index  matrix  (num__modes ,  2 ) 

the  starting  and  ending  state  for  each  mode 

the  damping  ratio  threshold  (below  threshold 

means  it's  a  disturbance) 

the  number  of  impulse  response  samples 

the  sampling  interval  (seconds) 


.  . .  Output  parameters : 

.  .  .  dist_modes  Indeces  of  modes  that  are  disturbances  (have 

damping  ratios  below  the  threshold  passed  into 
the  function  (damp_thresh)  ) . 

.  . .  mode_norms  Inner  product  of  total  pulse  response  and  modal 

pulse  response  -  gives  the  correlation/ 
contribution  of  each  mode  to  the  total  output. 


%  ...  The  number  of  inputs,  outputs,  and  states 

n_out  =  size (Cm,  1)  ;  %  The  number  of  rows  of  C 

n_in  =  size(Bm,  2)  ;  %  The  number  of  columns  of  B 

n_state  =  length (Am)  ; 

%  ...  Set  the  direct  transmission  matrix  to  zero 
D_mode  =  zeros  (num_modes*n_out ,  n__in  )  ; 

%  ...  Calculate  the  modal  output  matrix 

C_mode  =  zeros  (num_modes*n_out ,  n_state)  ; 

for  i  =  1 :num_modes 

idx  =  n_out*(i-l)  +  1  ; 

C_mode (idx:n_out*i  ,  m_idx (i,  1)  :m_idx (i , 2 )  )  =  Cm(l:n__out, 
m_idx ( i , 1 ) : m_idx ( i , 2 ) )  ; 
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end 


mode_norms  =  zeros (num_modes ,  1 )  ; 
for  i  =  l:n_in 


%  .  The  modal  impulse  responses 

y  =  dimpulse (Am, Bm,C_mode, D_mode, i,n_imp)  ; 

%  .  The  total  impulse  response 

y_t=  dimpulse  (Am,  Bm, Cm,  Dm,  i,n_imp)  ; 

%  .  Sum  the  impulse  responses  for  each  mode 


for  j  =  l:num_modes 

idx  =  n_out*(j-l)  +  1  ; 
for  jj  =  l:n_out 

y2 (idx+j j-1)  =  y_t ( : ,  j  j  )  '  *  y (:, idx+j j -1 )  ; 

end 

mode_norms  ( j  )  =  mode_norms  ( j  )  +  sum (y2  ( idx :  idx+n_out-l )  ) 

%  mode_norms  ( j  )  =  mode_norms  ( j  )  +  max  (y2  ( idx:  idx+n_out-l )  ) 


end 


end 

mode_norms=mode_norms  1  ; 
clear  idx 

%  ...  Sort  modes  from  min  to  max  damping  ratio 
[sorted_zeta, idx] =sort (abs (zeta) ) ; 
sorted_wn=wn(idx) ; 

%  ...  Find  the  indeces  of  those  below  the  damping  threshold  -  assume 
%  ...  no  first  order  modes  will  meet  the  criteria. 
dist_modes= [ ] ; 
i=l; 

current_zeta=sorted_zeta (i) ; 

while  (current_zeta  <  damp_thresh) 
dist_modes= [dist_modes  idx(i)]; 
i=i+l; 

current_zeta=sorted_zeta (i) ; 

end 
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DISTURBANCE  MODE  ANALYSIS 


function  [dist_modes , not__controlled]  = .  .  . 

analyze_eta_model  (wn,  zeta,  damp__thresh,  max_freq) 

%  analyze_eta_model .m  Stephen  Edwards  Jul  1999 


% — 

%  . . 

.  Input  parameters : 

%  . . 

.  wn 

The  natural  frequencies 

%  . . 

.  zeta 

The  modal  damping  ratios 

%  . . 

.  damp_thresh 

The  damping  ratio  threshold  (below  threshold 

% 

%  .. 

.  max__freq 

means  it's  a  disturbance) 

The  maximum  number  of  frequencies  that 

can  be 

% 

% 

%  . . 

.  Output  parameters 

handled  by  the  processor 

%  . . 

.  dist_modes 

Indeces  of  modes  that  are  disturbances 

(have 

% 

%  . . 

.  not_controlled 

damping  ratios  below  the  threshold) 
Modes  that  qualify  as  disturbances  but 

are  not 

% 

% — 

controlled  (due  to  excessive  number  of 

them) 

%  ...  Sort  modes  from  min  to  max  damping  ratio 
[sorted_zeta, idx] =sort (abs (zeta) ) ; 

%  ...  Find  the  indeces  of  those  below  the  damping  threshold  -  assume 
%  ...  no  first  order  modes  will  meet  the  criteria. 
dist_modes= [ ] ; 
i=l; 

current_zeta=sorted_zeta (i) ; 

while  (current_zeta  <  damp_thresh) 
dis t_modes= [ dis t_modes  idx ( i ) ] ; 
i=i+l; 

current_zeta=sorted_zeta ( i ) ; 

end 

%  ...  Sort  identified  modes  from  lowest  to  highest  frequency 
%  (to  prevent  flip-flopping) 

[sorted__wn,  idx]  =sort  (wn  (dist_modes)  )  ; 
dist_modes==dist_modes  (idx) ; 

%  ...  Only  take  the  number  of  modes  that  the  processor  can  handle 
if  length (dis t_modes)  >  max_freq 

dist„modes  =dist__modes  (1  :max_freq)  ; 

not_controlled  =dist_modes (max_freq+l : length (dist_modes) ) ; 
else 

not_controlled  =[]; 

end 
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DISTURBANCE  GENERATION 


/* 

*  dist_gen.c  Generates  time  varying  disturbances. 

*  The  parameters  are  initialized  in  the  Simulink 

*  initialization  file  (i.e.  " init_id_mimo" ,  etc.) 

* 


* 


*  Passed  Parameters: 

*  FLAG_DISTURB  = 

*  FLAG_DIST_VAR  = 

*  DIST_AMP 

*  AMP_RATE 

*  DIST_FREQ 

*  FREQ_RATE 

*  PHASE 

*  DT 


Logic  flag  (1  =  enable  disturbances) 

Logic  flag  (1  =  enable  disturbance  time-variaton) 
The  initial  amplitude (s)  of  the  disturbance (s) 
Rate  of  change  of  the  amplitude (s) 

The  initial  frequency (ies)  of  the  disturbance (s) 
Rate  of  change  of  the  frequency (ies) 

The  phase  angles  of  the  dist.  sinusiod(s) 

Sample  time  (step  size) 


*  Other  Variables: 

*  NUM_DIST  =  The  number  of  disturbance  frequencies 

* 


*/ 


# define  S_J?UNCT I ON_NAME  dist__gen 
#def ine  S_FUNCTION_LEVEL  2 


#include  "simstruc .h" 


# define 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 

#def ine 

#def ine 
#define 

#def ine 
#def ine 


FLAG_DISTURB(S) 
FLAG_DIST_VAR(S) 
DIST_AMP (S) 
AMP_RATE(S) 
DIST_FREQ (S) 
FREQ_RATE ( S ) 

PHASE (S) 

DT  (S) 

ssGetSFcnParam ( S , 0 ) 
ssGet SFcnParam ( S , 1 ) 
ssGetSFcnParam (S, 2) 
ssGetSFcnParam ( S , 3 ) 
ssGetSFcnParam ( S , 4 ) 
ssGetSFcnParam ( S , 5 ) 
ssGetSFcnParam ( S , 6 ) 
ssGetSFcnParam ( S , 7 ) 

N_PARAM 

8 

TWO_PI 

NUM_DIST 

6.28318530717959 
mxGetN (DIST_FREQ (S) ) 

NUM_SECTIONS 

WK_TIMER 

5 

3*NUMJDIST 

static  void  mdllnitializeSizes (SimStruct  *S) 

{ 

int_T  num_rwork; 

num_rwork=3*NUM_DIST+l;  /*  See  ssSetNumRWork  below  */ 

ssSetNumSFcnParams (S,  N_PARAM) ; 
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if  (ssGetNumSFcnParams (S)  !=  ssGetSFcnParamsCount (S) ) 

{ 

/*  Return  if  expected  number  !=  actual  number  */ 
return; 

} 

ssSetNumContStates (S,  0) ; 
ssSetNumDiscStates (S,  0)  ; 

if  (  ! ssSetNumlnputPorts (S,  0))  return; 
if  ( ! ssSetNumOutputPorts (S,  2))  return; 
ssSetOutputPortWidth ( S ,  0 ,  1 ) ; 

/*  The  first  output  is  a  sum  of  all  the 
disturbances  generated  —  it  contains 
all  of  the  diturbance  frequencies.  */ 
ssSetOutputPortWidth (S,  1,  NUM_DIST) ; 

/*  The  second  output  is  a  vector  of  the 
actual  disturbance  frequencies.  */ 

ssSetNumSampleTimes (S,  1); 
ssSetNumRWork (S,  num_rwork) ; 

/*  RWork[0 . .n_dist-l] =Current  dist.  amplitudes. 
RWork[n_dist . .2*n_dist-l] =Current  dist.  f reqs . 
RWork[2*n_dist . ,3*n_dist-l] =Current  dist.  angles 
RWork[3*n_dist] =Amount  of  time  in  profile  section  */ 
ssSetNumlWorMS,  1)  ; 

/*  IWork [ 0 ] =Current  Section  Number  */ 
ssSetNumPWork(S,  0) ; 
ssSetNumModes (S,  0)  ; 
ssSetNumNonsampledZCs (S,  0)  ; 

ssSetOptions (S,  SS_OPTION_EXCEPTION_FREE_CODE) ; 

} 

/ *  Function :  mdllnitializeSampleTimes 


*  Abstract: 

*  This  function  is  used  to  specify  the  sample  time(s)  for  your 

*  S-function. 

*/ 

static  void  mdllnitializeSampleTimes (SimStruct  *S) 

{ 

real__T  dt; 

dt  =mxGetPr (DT (S) ) [0] ; 

ssSetSampleTime (S,  0,  dt)  ; 
ssSetOf f setTime (S,  0,  0.0); 

} 

#def ine  MDL_INITIALIZE_CONDITIONS 
#if  defined (MDL_INITIALIZE_CONDITIONS) 

/*  Note,  this  routine  will  be  called  at  the  start  of  simulation  and 

*  if  it  is  present  in  an  enabled  subsystem  configured  to  reset 

*  states,  it  will  be  call  when  the  enabled  subsystem  restarts 

*  execution  to  reset  the  states. 

*/ 
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static  void  mdllnitializeConditions (SimStruct  *S) 


int_T  i ; 

real_T  *amp 

real_T  *freq 

real_T  *phase 

real__T  * RWork 

int_JT  *IWork 


=mxGetPr (DIST— AMP (S) ) ; 
=mxGetPr (DIST_FREQ(S) ) ; 
=mxGetPr( PHASE (S) ) ; 
=ssGetRWork(S) ; 
=ssGetIWork (S)  ; 


for  (i=0  ;  i<NUM_DIST  ;  i++) 

{  /*  Initialize  the  amplitudes  of 

the  disturbance  frequencies  */ 

RWork [ i ] =amp [ i ] ; 

/*  Initialize  the  angles  of 
the  disturbance  frequencies 
to  their  corresponding  phase 
angles  */ 

RWork [i+NUM_DIST]=freq[i] ; 

/*  Initialize  the  angles  of 
the  disturbance  frequencies 
to  their  corresponding  phase 
angles  */ 

RWork [i+2*NUM_DIST] =phase [i] ; 

} 

RWork [WK_TIMER]  =0.0;  /*  Set  timer  to  zero  */ 

IWork[0]  =0;  /*  Counter  for  time-varying  profile 

that  keeps  track  of  the  time 
so  that  different  sections  of 
the  profile  can  have  different 
frequency  variation  rates  */ 

} 

#endif  /*  MDL_INITIALIZE_CONDITIONS  */ 

#undef  MDL_START  /*  Change  to  #define  to  add  function  */ 

#if  defined (MDL_START) 

static  void  mdlStart (SimStruct  *S) 

{ 

} 

#endif  /*  MDL_START  */ 


/* 

*  Function:  mdlOutputs 
*/ 
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static  void  mdlOutputs (SimStruct  *S,  int_T  tid) 

{ 


int_T 

int_T 

int_T 

int_T 

int_T 


i; 

f lag^disturb  =mxGetPr ( FLAG_DI STURB (S) ) [0] ; 

f lag_dist_var  =mxGetPr ( FLAG_DIST_VAR (S) ) [0] ; 

*IWork  =ssGetIWork (S) ; 

cur rentes  ec  t i on ; 


real_T 

real_T 

real_T 

real_T 

real__T 

real_T 

real_T 

real__T 

real_T 


section_duration  [NUM_SECTIONS]  = 

{1.075,4.0/1.0,4.0,100.0}; 
section_freq_rate  [NUM_SECTIONS]  = 

{0.0,12.5664,0.0,-0.6283,0.0}; 


t ime_in_s  ec  t i on ; 
dt 

*amp_rate 

*freq_rate 

*RWork 

*yl 

*y2 


=mxGetPr (DT (S) ) [0] ; 
=mxGetPr(AMP_RATE(S)  )  ; 

=inxGe  t  Pr  ( FREQ_RATE  ( S )  )  ; 
=ssGetRWork(S) ; 

=ssGetOutputPortRealSignal (S, 0) ; 
=ssGetOutputPortRealSignal (S, 1) ; 


/* 

*  If 
*/ 


the  disturbance  is  enabled,  update  and  output  disturbance  signal 

if  (flag_disturb  ==  1) 

{ 

switch  ( f  lag__dist_var ) 

/*  Update  the  amplitude,  frequency,  and  angle  if 
variation  enabled  or  if  the  time-varying 
profile  is  being  used  */ 

{ 

case(0) : 

{  /*  Update  only  the  angle  if  time 

variation  is  disabled  */ 


for  (i=0  ;  i<NUM_DIST  ;  i++) 

RWork[i+2*NUM_DIST]  +=  RWork [i+NUM_DIST] *dt ; 
break; 

} 


case (1) : 

{  /*  Use  linear  variation  amounts  in 

set_dist.m  to  adjust  the 
amplitude,  frequency,  and  angle  */ 


for  (i=0  ;  i<NUM_DIST  ;  i++) 

{ 


RWork [i] 

RWork [ i+NUM_DIST] 
RWork [ i+2 *NUM_DIST] 

} 

break; 


+=amp_rate [i] *dt ; 
+=freq_rate [i] *dt ; 

+= RWork [ i+NUM_DIST] *dt ; 
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case (2 )  : 

{  /*  Keep  amplitude  constant,  but  update 

frequency  using  the  pre-set  rates 
(by  profile  section)  defined  at  the 
beginning  of  this  file,  and  also 
update  angle.  This  is  only  valid 
for  single  disturbance  frequencies 
(i.e.,  i=0)  */ 

current_section  =IWork [ 0 ] ; 

RWork  [  0+NUM__DIST]  +  = 

section__freq_rate  [current_section]  *dt  ; 

RWork  [0  +  2  *NUM_DIST]  +=RWork  [  0+NUM_DIST]  *dt  ; 


time__in_section  =  RWork  [WK_TIMER]  ; 

time_in_sect ion+=dt ; 


if  { time__in_section  > 

section_duration [current_section] ) 

{ 

current„section  +=  1; 

time_in_section  -  0.0; 

} 


IWork [ 0 ] 

RWork [WK_TIMER] 


=current_section; 
= t  ime__in_s  ec  t  i  on  ; 


} 

}  /*  End  of  amplitude,  frequency,  and  angle  updates  */ 


/*  Calculate  disturbance  (one  signal  includes  all  freqs)  */ 
yl  [ 0 ] =0 . 0 ; 

for  (i  =  0  ;  i<NUM__DIST  ;  i++) 

{ 

/*  Keep  the  angle  between  -2*pi  and  2*pi  radians  */ 
if  {  RWork [i+2*NUM_DIST]  >  TWO_PI  ) 

{ 

RWork [ i+2 *NUM_DIST] -=TW0_PI ; 

} 

if  (  RWork [ i+2 *NUM_DI ST]  <  -TWO_PI  ) 

{ 

RWork [ i+2  *NUM_DIST] +=TW0_PI ; 

} 

/*  Output  disturbance  signal  */ 

yl[0]  +=  RWork[i] * sin (RWork [ i+2 *NUM_DI ST] ) ; 

/*  Output  true  disturbance  frequencies  */ 
y2[i]  =  RWork [i+NUM_DI ST ] ; 

} 
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else 

{ 


/*  Set  output  to  zero  if  disturbance  disabled  */ 


} 


yl[0]=0.0;  /*  Output  no  disturbance  */ 

for  (i=0  ;  i<NUM_DIST  ;  i++) 

y2[i]=0.0;  /*  Output  freqs  =  0  */ 


/*  Disturbance  calculations  complete  */ 


#define  MDL_UPDATE  /*  Change  to  #undef  to  remove  function  */ 

#if  defined (MDL_UPDATE) 

static  void  mdlUpdate (SimStruct  *S,  int_T  tid) 

{ 

} 

#endif  /*  MDL_UPDATE  */ 

#define  MDL_DERIVATIVES  /*  Change  to  #undef  to  remove  function  */ 

#if  defined (MDL_DERIVATIVES) 

static  void  mdlDerivatives (SimStruct  *S) 

{ 

} 

#endif  /*  MDL_DERIVATIVES  */ 

static  void  mdlTerminate (SimStruct  *S) 

{ 

} 

/* ============== ===============* 

*  Required  S-function  trailer  * 

*=============================* / 

#ifdef  MATLAB_MEX_FILE  /*  Is  this  file  being  compiled  as  a  MEX-file?  */ 
#include  "simulink.c"  /*  MEX-file  interface  mechanism  */ 

#else 

#include  "cg_sfun.h"  /*  Code  generation  registration  function  */ 
#endif 
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DISTURBANCE  EFFECT  CALCULATION 


/* 

*  eta.c  S-function  to  calculate  the  disturbance  effect 

*  from  input-output  data  and  user  supplied  model  coefficients 

* 

* 

*/ 


#def ine  S_FUNC T I ON_NAME  eta 
#def ine  S_FUNCTION_LEVEL  2 
#include  "simstruc .h" 

ssGetSFcnParam ( S ,  0 ) 
ssGetSFcnParam ( S ,  1 ) 
ssGetSFcnParam  ( S ,  2 ) 
3 


#def ine  THETA_MATRIX ( S) 
ttdefine  P_MOD(S) 
ttdefine  DT(S) 
ttdefine  N_PARAM 


static  void  mdllnitializeSizes (SimStruct  *S) 

{ 

int  _T  p  =mxGet Pr ( P__MOD ( S ) ) [ 0 ] ; 

/*  order  of  ARX  model  */ 

int_T  pq^pm  =mxGetM (THETA_MATRIX ( S) ) ; 

/*  coefficient  matrix  row  dimension  =  p*q  +  p*m  */ 

int  _T  q  =mxGetN (THETA_MATRIX ( S) ) ; 

/*  coefficient  matrix  column  dimension  =q  */ 

int_T  m; 


m= (pq  pm/p) -q; 

ssSetNumSFcnParams (S,  N_PARAM) ; 

if  (ssGetNumSFcnParams (S)  !=  ssGetSFcnParamsCount (S) ) 

{ 

/*  Return  if  expected  number  !=  actual  number  */ 
return; 

} 

ssSetNumContStates (S,  0)  ; 
ssSetNumDiscStates (S,  0)  ; 


if  ( ! ssSetNumlnputPorts (S,  2))  return; 
ssSetInputPortWidth(S,  0,  m) ; 

/*  1st  Input  =  Inputs  (to  Actuators)  */ 

ssSetInputPortWidth(S,  1,  q) ; 

/*  2nd  Input  =  Outputs  (from  Sensors)  */ 

ssSetInputPortDirectFeedThrough(S,  0,  1) ; 
ssSet Input PortDirectFeedThrough (S,  1,  1) ; 
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if  ( !ssSetNumOutputPorts(S,  1))  return; 
ssSetOutputPortWidth ( S ,  0,  q) ;  /*  Dist.  Effect  */ 

ssSetNumSampleTimes (S,  1); 
ssSetNumRWork(S,  pg  pm) ; 
ssSetNumlWork ( S ,  0 )  ; 
ssSetNumPWork(S,  0)  ; 
ssSetNumModes (S,  0)  ; 
ssSetNumNonsampledZCs ( S ,  0 )  ; 

SSSetOptions (S,  SS_OPTION_EXCEPTION_FREE_CODE) ; 

} 

static  void  mdllnitializeSampleTimes (SimStruct  *S) 

{ 

real_T  dt  =mxGetPr(DT(S) ) [0] ; 


ssSetSampleTime (S,  0,  dt)  ; 
ssSetOf fsetTime (S,  0,  0.0); 


#def ine  MDL_INITIALIZE_CONDITIONS 
#if  defined (MDL_INITIALIZE_CONDITIONS) 
static  void  mdllnitializeConditions (SimStruct  *S) 
{ 

int  i; 

real_T  *RWork  =ssGetRWork(S) ; 


for  (  i  =  0  ;  i  <  ssGetNumRWork(S)  ;  i++  ) 

{ 

RWork [ i ]  =  0.0; 

) 

} 

#endif  /*  MDL_INITIALIZE_CONDITIONS  */ 


/*  Function:  mdlOutputs 

*  In  this  function,  you  compute  the  outputs  of  your  S-function 

*  block.  Generally  outputs  are  placed  in  the  output  vector,  ssGetY(S). 
*/ 

static  void  mdlOutputs (SimStruct  *S,  int_T  tid) 

{ 


real_T 

/* 

int_T 

/* 

int_T 

/* 

int_T 

/* 


*  theta  =mxGetPr ( THETA_MATRIX ( S ) ) ; 

pointer  to  the  model  coefficients  matrix  structure 

p  =mxGetPr ( P_MOD ( S ) ) [ 0 ] ; 

order  of  ARX  model  */ 

pq_pm  =mxGetM ( THETA_MATRIX ( S ) ) ; 

coefficient  matrix  row  dimension  =  p*q  +  p*m  */ 

q  =mxGetN ( THETA_MATRIX ( S ) ) ; 

coefficient  matrix  column  dimension  =q  */ 


*/ 
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int_T 


i,  j,  m; 


InputRealPtrsType  ul  =ssGetInputPortRealSignalPtrs (S, 0)  ; 

InputRealPtrsType  u2  =ssGetInputPortRealSignalPtrs (S, 1)  ; 

real_T  *y  =ssGetOutputPortRealSignal (S,  0) ; 

real_T  *Rwork  =ssGetRWork(S) ; 


The  RWork  vector  indeces  are  set  up  as  follows: 

[0]  =  yl(k-l)  1st  sensor  output,  delayed  by  one  time  step 

[1]  =  y2 (k-1 )  2nd  sensor  output,  "  "  " 

i 

[q-l]  =  yq(k-l)  qth  sensor  output,  "  "  " 

[q]  =  yl(k-2)  1st  sensor  output,  delayed  by  two  time  steps 

I 

[2*q-l] =  yq(k-2)  qth  sensor  output,  "  "  " 


[p*q-l]=  yq(k-p)  qth  sensor  output,  delayed  by  p  time  steps 


[p*q]  =  ul(k-l)  1st  actuator  input,  delayed  by  one  time  step 

[p*q+l]=  u2(k-l)  2nd  actuator  input,  *  "  " 

I 

[p*q+m-l]  =  um(k-l)  mth  actuator  input,  "  "  " 

[p*q+m]  =  ul(k-2)  1st  actuator  input,  delayed  by  two  time  steps 

I 

[p*q+2*m-l]  =  um(k-2)  mth  actuator  input,  "  "  " 


[p*q+p*m-l]  =  um(k-p)  mth  actuator  input,  delayed  by  p  time  steps 


m= (pq  pm/p) -q; 

/*  Shift  the  memory  */ 

for  (i  -  pq  pm-1  ;  i  >  p*q+m-l  ;  i--  ) 
RWork [i]  =  RWork [ i -m] ; 

for  (i  =  p*q-l  ;  i  >  q~l  ;  i —  ) 

RWork [ i ]  =  RWork [ i-q] ; 

/*  Save  the  current  input  */ 

for  (i  =  0  ;  i  <  q  ;  i++  ) 

RWork [i]  =  *u2 [ i] ; 

for  (i  =  0  ;  i  <  m  ;  i++  ) 

RWork[p*q+i]  =  *ul[i]; 


/* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

* 

*/ 
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/*  Calculate  the  disturbance  effect  —  eta  */ 

for  (  i  =  0  ;  i  <  q  ;  i++  ) 

{ 

y [i]  =  0.0; 

for  <  j  =  0  ;  j  <  pq_pm  ;  j++  ) 

{ 

y[i]  +=  theta [i*pq_pm+j ]  *  RWork[j]; 

} 

} 

} 


#define  MDL_UPDATE  /*  Change  to  #undef  to  remove  function  */ 

#if  defined (MDL_UPDATE) 

static  void  mdlUpdate (SimStruct  *S,  int_T  tid) 

{ 

} 

#endif  /*  MDL_UPDATE  */ 

#define  MDL_DERIVATIVES  /*  Change  to  #undef  to  remove  function  */ 

#if  defined (MDL_DERIVATIVES) 

static  void  mdlDerivatives (SimStruct  *S) 

{ 

} 

#endif  /*  MDL_DERIVATIVES  */ 

static  void  mdlTerminate (SimStruct  *S) 

{ 

} 

/*  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  =  !=  =  =  =  =  =  * 

*  Required  S-function  trailer  * 

*=============================* / 

#ifdef  MATLAB_MEX_FILE  /*  Is  this  file  being  compiled  as  a  MEX-file?  */ 
ttinclude  "simulink.c"  /*  MEX-file  interface  mechanism  */ 

#else 

#include  "cg_sfun.h"  /*  Code  generation  registration  function  */ 
#endif 
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CONTROL  CALCULATION  -  SINE/COSINE  METHOD 


/*  u_ff.c 
★ 

*  S-function  to  calculate  the  feedforward  control  signals 

*  using  the  disturbance  effect  and  the  disturbance-free  model. 

*  Recursive  Least  Squares  is  used  to  converge  on  the  feedforward 

*  control  signals'  sine  &  cosine  coefficients. 

* 

*  Some  of  the  elements  of  this  S-Function  were  modeled  after 

*  or  taken  from  code  developed  by  Neil  Goodzeit  (formerly  at  Princeton 

*  University)  in  his  file  named  "ff _f as t.c",  but  were  enhanced 

*  to  allow  more  than  two  control  inputs. 

* 

*  David  Marco  (NPS)  supplied  a  C-code  routine  to  do  matrix  inversion 

*  using  the  LU  decompostion  method. 

*/ 

#def ine  S_FUNCTION_NAME  u_ff 
#def ine  S_FUNCTION_LEVEL  2 


#include  " simstruc . h" 


# define  FLAG_CONTROL ( S ) 
#def ine  FLAG_UPDATE ( S ) 
#def ine  FLAG_FILTER ( S ) 
#def ine  BETA ( S ) 

#def ine  P_INIT(S) 

#def ine  LAMBDA (S) 

#def ine  DT(S) 

#define  M(S) 

#def ine  NFREQ(S) 

#def ine  FREQ(S) 


ssGet SFcnParam ( S ,  0 ) 
ssGetSFcnParam ( S , 1 ) 
ssGetSFcnParam ( S ,  2 ) 
ssGetSFcnParam ( S , 3 ) 
ssGetSFcnParam (S, 4)  /* 

ssGetSFcnParam ( S ,  5 )  /  * 
ssGetSFcnParam ( S , 6 ) 
ssGetSFcnParam ( S ,  7 )  /  * 
ssGetSFcnParam ( S ,  8 ) 
ssGetSFcnParam ( S ,  9 ) 


Covariance  */ 
Forgetting  Factor  */ 

Number  of  Inputs  */ 


#def ine  N_PARAM 
# define  MAX_COEF 

#def ine  MAX_FREQ 

# define  MAX_2_FR 

# define  MAX_CNTL 

#def ine  WK_COEF 

#def ine  WK_ANGL 

# define  WKJSC 


10  /*  Number  of  S-Function  parameters  */ 

36  /*  Maximum  number  of  feedforward 

coefficients  =  2 *m*MAX_FREQ  */ 

3  /*  Maximum  number  of  controlled 

frequencies  */ 

6  /*  Two  times  the  max  number  of 

frequencies  */ 

6  /*  Maximum  number  of  feedforward 

controls  */ 

1296  /*  The  work  vector  location  of  the 

feedforward  coefficients 
(MAX_C0EFA2  elements)  */ 

1332  /*  The  work  vector  location  of  the 

feedforward  angles 
(MAX_FREQ  elements )  * / 

1335  /*  The  work  vector  location  of  the  sines 

and  cosines  (2*p*NFREQ  elements)  */ 
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#def ine  N_IWORK  2  /*  Number  of  integer  work  vector 

elements  */ 

#def ine  TWO_PI  6.283185307 


static  void  mdllnitializeSizes (SimStruct  *S) 

{ 

int__T  p,  n_in,  n_outl,  n_out2,  n_out3,  n_work; 
int_T  nctl  =mxGetPr  (M(S) )  [0]  ;  /*  The  number  of  ff  controls  */ 

int_T  q  =mxGetM (BETA (S) ) ;  /*  Number  of  rows  of  Beta  =  q  */ 

int_T  n_col  =mxGetN ( BETA ( S ) ) ;  /*  Number  of  columns  of  Beta  */ 

p  =  n_col/nctl;  /*  ARX  model  order  */ 

n_in  =  q; 

n_outl  =  nctl;  /*  feeforward  signals  */ 

n_out2  =  q;  /*  residuals  */ 

n_out3  =  MAX_COEF;  /*  feedforward  coefficients  */ 

n_work  = 

MAX_COEF*MAX_COEF  +  MAX_COEF  +  MAX_FREQ  +  2*p*MAX_FREQ; 
ssSetNumSFcnParams (S,  N_PARAM ) ; 

if  (ssGetNumSFcnParams (S)  !=  ssGetSFcnParamsCount (S) ) 

{ 

/*  Return  if  expected  number  !=  actual  number  */ 
return; 

} 

ssSetNumContStates (S,  0) ; 
ssSetNumDiscStates (S,  0)  ; 

if  ( ! ssSetNumlnputPorts (S,  1))  return; 
ssSetInputPortWidth(S,  0,  n_in) ; 

/*  Input  =  Disturbance  Effect  (q  channels)  */ 
ssSetInputPortDirectFeedThrough{S,  0,  1) ; 

if  (! ssSetNumOutput Ports (S,  3))  return; 
ssSetOutputPortWidth ( S ,  0 ,  n_out 1 ) ; 
ssSetOutputPortWidthfS,  1,  n_out2) ; 
ssSetOutputPortWidth (S,  2,  n_out3) ; 
ssSetNumSampleTimes (S,  1) ; 
ssSetNumRWork(S,  n_work) ; 
ssSetNumlWork ( S ,  N_IW0RK) ; 
ssSetNumPWork(S,  0); 
ssSetNumModes (S,  0); 
ssSetNumNonsampledZCs (S,  0) ; 

ssSetOptions (S,  SS_OPTION_EXCEPTION_FREE_CODE) ; 


/*  Function:  mdllnitializeSampleTimes 

*  This  function  is  used  to  specify  the  sample  time(s)  for  your 

*  S-f unction.  You  must  register  the  same  number  of  sample  times  as 

*  specified  in  ssSetNumSampleTimes. 
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static  void  mdllnitializeSampleTimes (SimStruct  *S) 

{ 

real__T  tstep  =mxGe tPr  ( DT ( S )  )  [0]  ; 

ssSetSampleTime (S,  0,  tstep); 
ssSetOf f setTime  (S,  0,  0.0); 

} 

# define  MDL__INITIALIZE_CONDITIONS 
#if  defined (MDL_INITIALIZE_CONDITIONS) 

static  void  mdllnitializeConditions (SimStruct  *S) 

{ 

int_T  i ; 

int__T  *IWork  =ssGetIWork (S) ; 

real_T  *RWork  =ssGetRWork (S) ; 

real_T  p__init  =mxGetPr (P_INIT (S) ) [0] ; 

/* 

*  Initilize  covariance  matrix,  feedforward  coef, 

*  and  feedforward  angles 
*/ 


/*  Zero  the  real  work  vector  elements  */ 

for  (  i  =  0  ;  i  <  ssGetNumRWork (S)  ;  i++  ) 
RWork [ i ]  =  0.0; 

/*  Initialize  the  cov  matrix  diagonal  elements  */ 
for  (  i  =  0  ;  i  <  MAX_COEF  ;  i++  ) 

RWork [i+MAX_COEF*i]  =  p_init; 

/*  Zero  the  integer  work  vector  elements  */ 

for  (  i '=  0  ;  i  <  ssGetNumlWork (S)  ;  i++  ) 

IWork[i]  =  0; 

} 

#endif  /*  MDL_INITIALIZE_CONDITIONS  */ 


/*  Function:  mdlOutputs 

*  In  this  function,  you  compute  the  outputs  of  your  S-function 

*  block. 

*/ 

static  void  mdlOutputs  (SimStruct  *S,  int__T  tid) 

{ 


/***************  Type  declarations 


InputRealPtrsType 

u 

real_T 

*y_cont 

real_T 

*y_res 

real_T 

*y_coef 

*******************★**************/ 

=ssGetInputPortRealSignalPtrs (S, 0) ; 
=ssGetOutputPortRealSignal ( S, 0 ) ; 
=ssGetOutputPortRealSignal (S, 1) ; 
=ssGetOutputPortRealSignal (S, 2) ; 
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real_T 

*RWork 

=ssGetRWork(S) 

/ 

real_T 

*freq 

=mxGetPr (FREQ(S) ) 

7 

real_T 

*beta 

=mxGetPr ( BETA ( S ) ) 

7 

real_T 

lambda 

=mxGetPr ( LAMBDA ( S ) 

)  [0] 

real_T 

tstep 

=mxGetPr (DT (S) ) [0] 

/ 

int_T 

*IWork 

=ssGetIWork (S) 

/ 

int_T 

n_row 

=mxGetM ( BETA ( S ) ) 

t 

int_T 

n__col 

=mxGetN ( BETA ( S ) ) 

7 

int_T 

nctl 

=mxGetPr (M(S) ) [0] 

t 

int_T 

nfreq 

=mxGetPr (NFREQ ( S) ) [ 0 ] 

int_T 

on_of f 

=mxGetPr ( FLAG_CONTROL ( S ) ) [ 0 ]  ; 

int_T 

enable 

=mxGetPr ( FLAG_UPDATE ( S ) ) [ 0 ] 

int_T 

enable_upd 

7 

int__T 

counter 

;  /* 

The  enable  counter  */ 

int_T 

i ,  j,  k,  1, 

m  ;  /* 

Loop  indices  */ 

int_T 

idx,  idx_c, 

idx_s ,  idx_f ;  / * 

Work  indexes  */ 

int_T 

P 

;  /* 

The  ARX  model  order  */ 

/ *  Temporary 

variables  */ 

realJT 

angle  [MAX_2. 

_FR]  ;  /* 

ff  sines/cosines  */ 

real__T 

coef [MAX_CNTL] [MAX_C0EF] ;  /* 

The  coefficient  matrix 

for  the  feedforward 

recursion  */ 

real_T 

f  f__angle 

;  /* 

The  ff  angle  */ 

real__T 

c__angle_k 

;  /* 

The  cosine  of  shifted 

feedforward  angle  */ 

real_T 

s_angle_k 

;  /* 

The  sine  of  shifted 

feedforward  angle  */ 

real__T 

c„temp 

;  /* 

Temporary  storage  for 

cosine  */ 

real_T 

s_temp 

;  /* 

Temporary  storage  for 

sine  */ 

real_T 

y_k [MAX_CNTL] 

real_T 

res [MAX_CNTL] 

real_T 

gain [MAX_C0EF] [MAX_CNTL] ;  /* 

The  update  gains  */ 

real___T 

temp [MAX_C0EF ] [MAX_CNTL] ;  /* 

P*coef  */ 

real_T 

det 

7 

real_T 

p_del 

;  /* 

The  update  to  cov 

matrix  elements  */ 

int__T 

ncoef 

7 

/*  Variables  for  matrix  inversion  */ 


int_T 

real_T 

real_T 

real_T 


n,  ki,  sing_flag  ; 

b,  bl,  b2 

a_local [MAX_CNTL+1] [MAX_CNTL+1] ;  /*  [7] [7]  */ 

ainv [MAX_CNTL+1 ] [MAX_CNTL+1] ;  /*  [7] [7]  */ 
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/*  Determine  the  ARX  model  order  */ 
p  =  n_col  /  nctl  ; 


/***********  Evaluate  the  enable  logic 


IWork  =  ssGetlWork(S) ;  /*  pointer  to  integer  work  vector  */ 

enable_upd  =  IWork [0]  ; 

counter  =  IWorkfl]  ; 

if  (  enable  !=  enable_upd  ) 

{ 

if  (enable  ==  0  ) 

{ 

enable_upd  =  0  ;  /*  Disable  the  coefficient  updates 

immediately  when  commanded  */ 
counter  =  0  ;  /*  Reset  enable  delay  counter  */ 

} 

if  (enable  ==  1  ) 

{ 

if  (  counter  >  p  ) 


enable_upd  =  1  ; 

/* 

Enable  logic  after  a 
p  step  delay  */ 

else 

} 

} 

counter  +=  1  ; 

/* 

Otherwise  increment 
the  counter  */ 

/* 

End  of  enable  logic  */ 

IWork [0]  =  enable_upd  ; 
IWork[l]  =  counter  ; 


/***********  zero  the  feedforward  equation  coefficients  ************/ 
for  (  i  =  0  ;  i  <  MAX_CNTL  ;  i++  ) 

{ 

for  (  j  =  0  ;  j  <  MAX_COEF  ;  j++  ) 

coef[i][j]  =  0.0  ;  /*  Zero  the  feedforward 

equation  coefficients  */ 


} 


for  ( 
{ 


Calculate  the  feedforward  angles  *************************/ 
i  =  0  ;  i  <  nfreq  ;  i++  ) 


idx_f  =  2  *  i  ; 

f f_angle  =  RWork [WK_ANGL  +  i]  ; 

ff_angle  +=  freq[i]  *  tstep; 

if  (  f f_angle  >  TWO_PI  ) 

ff_angle  -=  TWO_PI; 
angle[idx_f  ]  =  cos (f f_angle) ; 

angle[idx_f  +1]  =  sin (f f_angle) ; 

RWork [WK_ANGL  +  i]  =  ff_angle; 
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/******* 


Calculate  the  linear  equation  coefficients  ***************/ 


idx_c  =  2  *  i  *  nctl;  /*  starting  column  index  in  coef 

matrix  for  cosine  terms  */ 

idx_s  =  idx_c  +  nctl;  /*  starting  column  index  in  coef 

matrix  for  sine  terms  */ 

for  (  k  =  0  ;  k  <  p  ;  k++  ) 

{ 

if  {  k  !=  0  ) 

{ 

c_temp  =  c_angle_k; 
s_temp  -  s__angle_k; 

} 

if  (  k  ==  0  ) 

{ 

c_temp  =  angle [idx_f  ]; 
s_temp  =  angle [idx_f+l] ; 

} 

c__angle_k  =  RWork[WK__SC  +  k  +  idx_f*p  ];  /*  Retrieve 

cosines  from 
the  table  */ 

s_angle__k  =  RWork[WK_SC  +  k  +  (idx_f +1)  *p]  ;  /*  Retrieve 

sines  from 
the  table  */ 

RWork[WK_SC  +  k  +  idx_f*p  ]  =  c_temp;  /*  Shift  the 

cos  table  */ 

RWork[WK_SC  +  k  +  (idx_f+l)*p]  =  s__temp;  /*  Shift  the 

sine  table  */ 

idx  =  k  *  nctl  ;  /*  starting  column  index  for  beta  matrix  */ 

for  (  j  =  0  ;  j  <  n_row  ;  j++  ) 

{ 

for  (  1  =  0  ;  1  <  nctl  ;  1++  ) 

{ 

coef [ j ] [idx_c+l]  +=  beta [ j+ (idx+1) *n_row]  *  c_angle_k; 
coef [ j ] [idx_s+l]  +=  beta [ j + ( idx+1) *n_row]  *  s_angle_k; 

} 

} 

}  /*  End  loop  for  p  */ 

}  /*  End  loop  for  nfreq  */ 


207 


/************ 


Update  the  feedforward  control  coefficients'  **********/ 


/***************** 
ncoef  =  nctl  * 


Calculate  the  eta  residual  **********************/ 
2  *  nfreq  ;  /*  Number  of  feedforward 

coefficients  to  solve  for  */ 


for  (  i  =  0  ;  i  <  n_row  ;  i++  ) 

{ 

y_k[i]  =  0.0; 

for  (  j  =  0  ;  j  <  ncoef  ;  j++  ) 

y_k[i]  +=  coef [i] [ j ]  *  RWork [WK_C0EF  +  j]; 

/*  The  expected  value  of  the  disturbance  effect  */ 

res[i]  =  *u[i]  -  y_k[i];  /*  Calculate  the  residuals  */ 

y_res[i]  =  res[i];  /*  Output  the  residuals  */ 


/************  Calculate  the  update  gain  matrix 
for  (  i  =  0  ;  i  <  ncoef  ;  i++  ) 


for  (  j  =  0  ;  j  <  n_row  ;  j++  ) 

{ 

temp [i] [j]  =  0.0; 

for  (  k  =  0  ;  k  <  ncoef  ;  k++  ) 

tempti] [j]  +=  RWork[i  +  MAX_COEF*k]  *  coef[j] [k]  ; 

/*  P*fi  =  P*coef'  (ncoef  x  n_row)  */ 


} 


} 


/*  Prepare  matrix  "a_local"  (fi'*P*fi  +  lambda*eye (n_row,  n_row) )  for 

*  inversion.  This  routine  uses  indeces  from  l..n  (as  opposed  to 

*  0..n-l),  so  the  first  row  and  column  will  be  zeroed  out  before 

*  proceding.  */ 


for  (  i  =  0  ;  i  <=  n_row  ;  i++  ) 
a_local [0] [i]  =  0.0; 

for  (  i  =  1  ;  i  <=  n_row  ;  i++  ) 
a_local [i] [0]  =  0.0; 

for  (  i  =  1  ;  i  <=  n_row  ;  i++  ) 

{ 

for  (  j  =  1  ;  j  <=  n_row  ;  j++  ) 

{ 

a_local [i] [ j ]  =  0.0; 

if  (i  ==  j) 

a_local [i] [ j]  =  lambda; 

for  (  k  =  0  ;  k  <  ncoef  ;  k++  ) 

a_local [i] [ j ]  +=  coef[i-l][k]  *  temp [k] [ j -1] ; 

/*  fi'*P*fi  +  lambda*eye (n_row,  n_row)  */ 

/*  =  coef *P*coef 1 +lambda*eye (n_row,  n_row)  */ 

} 

} 
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/* 

* 


* 

*/ 


Calculate  matrix  inverse  and  update  gain  only  if  logic  is  enabled. 
The  matrix  being  inverted  is  equivalent  to: 

inv(fi'*P*fi  +  lambda*eye (n_row,  n_row) ) 


if  (  enable_upd  ==  1  ) 

{ 

sing_flag  =  0; 
n  =  n_row; 

for  (i=l;i<=n;++i) 

{ 

for  ( j=l ; j<=n; ++ j ) 

{ 

ainv[i] [ j]  =  0.0; 

} 

} 

for  (i=l;i<=n;++i) 

{ 

ainv[i] [i]  =  1.0; 

} 

for  (k=l;k<=n-l; ++k) 

{ 

b  =  a_local [k] [k] ; 
ki  =  k; 

for  (i=k+l; i<=n; ++i) 

{ 

if (  (fabs(b)  -  fabs (a_local [i] [k] ) )  >=  0.0  ) 

{ 

} 

else 

{ 

b  =  a_local[i] [k] ; 
ki  =  i; 

} 

} 

if (  fabs (b)  <  0.0001) 

{ 

sing_flag  =  1; 
break; 

} 

if (  (ki-k)  ==  0) 

{ 

) 

else 

{ 

for  ( j=k; j<=n;++j ) 
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{ 

bl  =  a_local [k] [ j ] ; 
a_local[k] [j]  =  a_local[ki] [j] ; 
a_local [ki] [ j ]  =  bl; 

} 

for  { j=l; j<=n;++j) 

{ 

b2  =  ainv[k] [ j ] ; 
ainv[k] [ j ]  =  ainv[ki] [j] ; 
ainv[ki] [ j ]  =  b2; 

} 


for  ( j=k+l; j<=n; ++j ) 

{ 

a_local [k] [ j ]  =  a_local [k] [ j ] /b; 

} 

for  (j=l? j<=n;++j) 

{ 

ainv[k][j]  =  ainv[k] [ j ] /b; 

} 

for  (i=k+l; i<=n; ++i) 

{ 

for  ( j=k+l; j<=n;++j) 

{ 

a_local [i] [ j]  =  a_local [i] [ j ]  - 

a_local [i] [k] *a_local [k] [ j ] ; 

} 

for  (j=l; j<=n;++j) 

{ 

ainv[i] [ j ]  =  ainv[i][j]  -  a_local [i] [k] *ainv[k] [ j ] ; 

} 

} 

} 

if(sing_flag  ==  0) 

{ 

for  (j=l; j<=n;++j) 

{ 

ainv[n][j]  =  ainv[n] [ j] /a_local [n] [n] ; 

} 

for  (k=n-l ; k>=l ; — k) 

{ 

for  ( j=l; j<=n;++j) 

{ 

for  (i=k+l;i<=n;++i) 

{ 

ainvtk] [j]  =  ainv[k][j]  -  a_local [k] [i] *ainv[i] [ j] 

} 

} 
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} 

} 

else 

{ 

/*  mexPrintf ( "Singular  or  Ill-Conditioned  Matrix\n");  */ 

} 


/* 

*  MATRIX  INVERSION  COMPLETE  * 

*  Now  that  Inversion  is  done,  continue  with  Gain  update 

*  (Remember  that  *ai  pointer  is  7x7,  not  6x6  (when  m=6) ) 
*/ 


for  (  i  =  0  ;  i  <  ncoef  ;  i++  ) 

{ 

for  (  j  =  0  ;  j  <  n_row  ;  j++  ) 

{ 

gain[i] [j]  =  0.0; 


for  (  k  =  0  ;  k  <  n_row  ;  k++  ) 

gain[i] [j]  +=  temp[i] [k]  *  ainv[k+l] [ j+1] ; 

/*  L=P*fi ' *inv(fi 1 *P*fi  +  lambda*eye (n_row,  n_row) )  */ 


} 


} 


/*  End  coefficient  update  enable  (starts  prior  to  matrix  inversion)  */ 


/*  Update  the  parameter  estimates  */ 
for  (  i  =  0  ;  i  <  ncoef  ;  i++  ) 

{ 

if  (  enable_upd  ==  1  ) 

{ 

for  (  j  =  0  ;  j  <  n_row  ;  j++  ) 

RWork [WK_C0EF  +  i]  +=  gain[i][j]  *  res[j]; 

}  '  /*  End  coefficient  update  enable  */ 

y_coef[i]  =  RWork [WK_C0EF  +  i] ; 

/*  Put  the  coefficients  in  the  output  vector  */ 

} 


for  (  i  =  ncoef  ;  i  <  MAX_C0EF  ;  i++  ) 
y_coef[i]  =  0.0; 


/************  update  the  covariance  matrix 
if  (  enable_upd  ==  1  ) 

{ 

for  (  i  =  0  ;  i  <  ncoef  ;  i++  ) 

{ 
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j  <  ncoef  ;  j++  ) 


for  (  j  =  i  ; 

{ 

p_del  =  0.0; 

for  (  k  =  0  ;  k  <  n_row  ;  k++  ) 

p_del  +=  gain[i] [k]  *  temp[j][k]; 

RWork [ i  +  MAX_COEF* j ]  -=  p_del; 

RWork [ i  +  MAX_COEF*j]  /=  lambda; 


if  (  i  !=  j  ) 

RWork [j  +  MAX_COEF*i]  =  RWork [i  +  MAX_C0EF*j] 


} 

} 

}  /*End  coefficient  update  enable  */ 

/****  Calculate  the  feedforward  control,  if  enabled  (else  =  0)  ****/ 
if  (  on_off  ==  1  ) 

{ 

for  (  i  =  0  ;  i  <  nctl  ;  i++  ) 

{ 

y_cont [ i ]  =  0 . 0 ; 

for  (  j  =  0  ;  j  <  2*nfreq  ;  j++  ) 

{ 

idx  =  j  *  nctl  +  i; 

y_cont[i]  +=  RWork [WK_COEF  +  idx]  *  angle[j]; 

} 

} 

} 

else 

{ 

for  (  i  =  0  ;  i  <  nctl  ;  i++  ) 

{ 

y_cont [ i ]  =  0.0; 

} 


}  /*******  End  of  mdlOutputs  *******/ 

# define  MDL_UPDATE 
#if  defined (MDL_UPDATE) 

static  void  mdlUpdate (SimStruct  *S,  int_T  tid) 

{ 

} 

#endif  /*  MDL_UPDATE  */ 

#define  MDL_DERIVATIVES  /*  Change  to  #undef  to  remove  function  */ 
#if  defined (MDL_DERI VAT IVES) 

static  void  mdlDerivatives (SimStruct  *S) 

{ 

} 

#endif  /*  MDL_DERIVATIVES  */ 
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static  void  mdlTerminate (SimStruct  *S) 

{ 

} 

/*=============================* 

*  Required  S-function  trailer  * 
*=============================*/ 


#ifdef  MATL AB_MEX_F I L E  /*  Is  this  file  being  compiled  as  a  MEX-file?  */ 
#include  "simulink.c"  /*  MEX-file  interface  mechanism  */ 

#else 

#include  "cg_sfun.h"  /*  Code  generation  registration  function  */ 
#endif 
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CONTROL  CALCULATION  -  ADAPTIVE  BASIS  METHOD 

/*  u_ff_eta.c 
* 

*  S-function  to  calculate  the  feedforward  control  signals 

*  using  the  Clear  Box  Adaptive  Basis  Method.  Recursive 

*  Least  Squares  is  used  to  converge  on  the  feedforward 

*  control  signals'  coefficients. 

* 

*  This  version  employs  filtering  of  the  eta  signal  to  remove 

*  frequencies  associated  with  modes  de-selected  for  control. 

* 

*  David  Marco  (NPS)  supplied  a  C-code  routine  to  do  matrix  inversion 

*  using  the  LU  decompostion  method. 

*/ 


#def ine  S_F UNC T I ON_NAME  u__ff_eta_N6 
# define  S__FUNCTION_LEVEL  2 
#include  "simstruc .h" 


#def ine 
#def ine 
#def ine 
#def ine 
#define 
#def ine 
#def ine 
#def ine 
# define 
# define 
#def ine 
#def ine 
#def ine 
#def ine 
#def ine 


FLAG_CONTROL(S) 
FLAG_UPDATE ( S ) 
FLAG_FILTER(S) 
BETA ( S ) 

P__INIT  ( S ) 

LAMBDA  (S) 

DT  ( S ) 

BASIS (S) 
FILT__ORDER  ( S ) 
FILT__ALPHA  ( S ) 
FILT_BETA(S) 
N__PARAM 
M 
Q 
N 


# define  MAX_DEL 


# define  N_COEF 
#def ine  WK_COEF 

#def ine  WK_ETA 


#def ine  WK_F I LT_ETA 
#def ine  N_IWORK 


ssGetSFcnParam ( S ,  0 ) 
ssGetSFcnParam ( S ,  1 ) 
ssGetSFcnParam ( S ,  2 ) 
ssGetSFcnParam ( S ,  3 ) 
ssGetSFcnParam ( S , 4 ) 
ssGetSFcnParam (S, 5) 
ssGetSFcnParam ( S ,  6 ) 
ssGetSFcnParam ( S ,  7 ) 
ssGetSFcnParam (S, 8) 
ssGetSFcnParam ( S , 9 ) 
ssGetSFcnParam(S, 10) 

11  /*  Number  of  S-Function  parameters  */ 

6  /*  m  =  Number  of  inputs  */ 

6  /*  q  =  Number  of  outputs  */ 

6  /*  N  =  Number  of  basis  sets  needed  = 

2*Number  of  controlled  frequencies  */ 

72  /*  Maximum  amout  of  delay  used  in  eta 

history  = 

L [N]  +  p_max  +2=  30  +40  +2  */ 

36  /*  Number  of  ff  coef f icients= (m*N)  */ 

1296  /*  The  work  vector  location  of  the  ff 

coefficients  (N__COEF^2)  */ 

1332  /*  Start  of  the  time  history  of  the 

disturbance  effect,  eta 
(N__C0EFA2  +  N_C0EF)  */ 

1764  /*  Start  of  the  time  history  of  the 

filtered  eta  signal  */ 

2  /*  Number  of  integer  work  vector  elems  */ 


/*  Function:  mdllnitializeSizes  */ 
static  void  mdllnitializeSizes (SimStruct  *S) 
{ 
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int_T  p,  n_in,  n_outl,  n_out2,  n_out3,  n_work 
int_T  pm  =mxGetN  ( BETA  ( S ) ) 

int__T  delay[N]  ={2,  7,  9,  12,  16,  22} 

int__T  l__max  =delay[N-l] 


p  =  pm/M 

n_in  =  Q 
n_outl  =  M 

n_out2  -  Q 


n_out3  =  N_COEF 

n_work  =  WK_F  I  LT_ETA  +  MAX__DEL 


/*  */ 


/*  Variable  Definitions 


* 

* 

delay 

* 

l_max 

* 

P 

* 

n__in 

* 

n__outl 

* 

n_out2 

* 

n_out3 

* 

n_work 

* 

* 

* 

* 

*/ 

Basis  time  shifts  -  defined  here  and  in  mdlOutputs 

Maximum  amount  of  time  history  delay 

ARX  model  order 

Number  of  inputs  to  S-function 

Number  of  elements  in  first  output  vector 

Number  of  elements  in  second  output  vector 

Number  of  elements  in  third  output  vector 

Number  of  work  vector  elements;  covariance, 

coefficients,  unfiltered  eta  time  histories  for  all 

six  struts,  and  filtered  scalar  eta  history  for  basis 

strut  -  see  comments  at  end  of  declarations  in 

mdlOutputs  function . 


ssSetNumSFcnParams (S,  N_PARAM) ; 

if  (ssGetNumSFcnParams (S)  !=  ssGetSFcnParamsCount (S) ) 

{ 

return;  /*  Return  if  expected  number  1=  actual  number  */ 

} 


ssSetNumContS tates ( S ,  0 ) ; 
ssSetNumDiscStates (S,  0) ; 


if  ( ! ssSetNumlnputPorts (S,  1))  return; 
ssSetInputPortWidth(S,  0,  n_in) ; 
ssSetInputPortDirectFeedThrough(S,  0,  1) ; 

if  ( I ssSetNumOutputPorts ( S ,  3))  return; 
ssSetOutputPortWidth(S,  0,  n_outl) ; 
ssSetOutputPortWidth(S,  1,  n_out2) ; 
ssSetOutputPortWidth(S,  2,  n_out3) ; 
ssSetNumSampleTimes ( S ,  1 ) ; 
ssSetNumRWork(S,  n_work) ; 
ssSetNumlWork  ( S ,  N_IW0RK); 
ssSetNumPWork(S,  0) ; 
ssSetNumModes (S,  0) ; 
ssSetNumNonsampledZCs (S,  0) ; 

SSSetOptions (S,  SS_OPTION_EXCEPTION_FREE_CODE) ; 
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/*  Function:  mdllnitializeSampleTimes  */ 
static  void  mdllnitializeSampleTimes (SimStruct  *S) 
{ 

real_T  dt  =mxGetPr (DT(S) )  [0]  ; 


ssSetSampleTime (S,  0,  dt)  ; 
ssSetOf f setTime (S,  0,  0.0); 


#def ine  MDL_INITIALIZE__CONDITIONS 

#if  defined (MDL_INITIALIZE_CONDITIONS) 

static  void  mdllnitializeConditions (SimStruct  *S) 


int_T 

int_T 

realJT 

real_T 


i; 

* IWork 
*RWork 
p__init 


=ssGetIWork(S) ; 
=ssGetRWork(S) ; 

=mxGet Pr ( P_INIT ( S ) ) [0] ; 


/* 

*  Initilize  the  cov  matrix,  feedforward  coef,  and  feedforward  angles 
*/ 


/*  Zero  all  real  work  vector  elements  */ 

for  (  i  =  0  ;  i  <  ssGetNumRWork (S)  ;  i++  ) 
RWork [ i ]  =  0.0; 


/*  Initialize  the  cov  matrix  diagonal  elements  */ 
for  (  i  =  0  ;  i  <  N__COEF  ;  i++  ) 

RWork [i+N_COEF*i]  =  p_init; 


/*  Zero  the  integer  work  vector  elements  */ 

for  (  i  =  0  ;  i  <  ssGetNumlWork (S)  ;  i++  ) 
IWork [ i ]  =  0 ; 

} 

#endif  /*  MDL_INITIALIZE_CONDITIONS  */ 


/*  Function:  mdlOutputs  */ 

static  void  mdlOutputs (SimStruct  *S,  int_T  tid) 

{ 

/***************  Ty9e  declarations  **********************************/ 


InputRealPtrsType 

u 

real__T 

*y_cont 

real_T 

*y_res 

real__T 

*y_coef 

=ssGetInputPortRealSignalPtrs ( S , 0 ) 
=ssGetOutputPortRealSignal (S, 0) ; 
=ssGetOutputPortRealSignal (S, 1) ; 
=ssGetOutputPortRealSignal (S, 2 ) ; 


real_T 

real_T 

real_T 

real_T 

real_T 


* RWork 
*beta 
lambda 
max_del ; 
dt 


=ssGetRWork(S)  ; 

=mxGetPr (BETA(S) ) 

=mxGetPr (LAMBDA (S) ) [0] 

/*  Maximum  delay  of  eta  history  */ 
=mxGetPr (DT (S) ) [0] 


int_T  * IWork  =ssGet IWork (S)  ; 

int_T  basis  =mxGetPr (BASIS (S) )[ 0]  ;  /*  Basis  strut  #  */ 
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/*  Time  shifts  */ 
/*  Max  delay  */ 


int_T  pm  =mxGetN ( BETA ( S ) ) 

int_T  delay[N]={2,  7,  9,  12,  16,  22} 
int_T  l_max  =delay[N-l] 

int_T  on_of  f  =mxGet Pr  ( FLAG__CONTROL  ( S )  )  [  0  ] 
int_T  enable  =mxGetPr  (FLAG_JJPDATE  ( S)  )  [  0  ] 

/*  The  externally  commanded  coefficient  update  enable  */ 

int_T  enable_upd; 

/*  The  internal  feedforward  coefficient  update  enable  */ 
int_T  counter; 

int_T  i,  j,  k,  1  ;  /*  Loop  indices  */ 

int_T  p  ;  /*  ARX  model  order  */ 

/*  Variables  for  eta  filtering  */ 

int__T  f  ilt_enable=mxGetPr  (FLAG_FILTER (S)  )  [0]  ; 

/*  The  enable  for  the  filtering  of  eta  */ 

int_T  filt_ord  =mxGetPr (FILT_ORDER(S) ) [0] ; 

/*  Order  of  ARX  model  for  eta  filter  */ 

real_T  *filt_beta  =mxGetPr (FILT_BETA(S) ) ; 

real__T  *f ilt_alpha=mxGetPr  (FILT_ALPHA (S)  )  ; 

/*  Pointers  to  filter's  ARX  model  coeff's  */ 

/*  Temporary  variables  *1 

real_T  Phi[N_COEF] [Q] 

real_T  eta_star [MAX_DEL+1 ] 

real_T  eta__hat  [M] 

real_T  res  [M] 

/*  residual  =  -eta  +  eta„hat 

real_T  gain[N_COEF] [Q] 

/*  The  update  gain  matrix  =  P*Phi*inv[Phi ' *P*Phi+R]  */ 

real_T  temp[N_COEF] [Q]  ;  /*  P*Phi  */ 

real_T  det  ; 

real_T  p_del  ; 

/*  The  update  to  each  covariance  matrix  element  */ 

/*  Variables  for  matrix  inversion  */ 
int_T  n,  ki,  sing_flag 

real_T  b,  bl,  b2 

real_T  a_local[Q+l] [Q+l] 

real_T  ainv[Q+l] [Q+l] 

/*The  RWork  vector  indeces  are  set  up  as  follows: 

*  RWork [0  to  N_COEFA2 -1 ]  —  The  feedforward  coefficient  cov  matrix 

*  (stored  following  Mat lab  convention) 

*  RWork [last  +1  to  last  +  N_COEF]  —  The  feedforward  coeff's 

*  RWork [last  +1  to  last  +  MAX_DEL*q]  —  The  eta  time  history 

*  RWork [last  +1  to  last  +  MAX_DEL]  —  History  of  filtered  eta 

*/ 


/*  [7] [7]  */ 
/*  [7]  [7]  */ 


;  /*  Phi  matrix  */ 

;  /*  Scalar  eta  history  */ 

;  /*  Estimated  eta  */ 

# 

=  y  -  y_hat  (  y  =  -eta  )  */ 


************ / 


/*  Determine  the  ARX  model  order  */ 
p  =  pm  /  M  ; 

/***********  Evaluate  the  enable  logic 
IWork  =  ssGetlWork(S) 

enable_upd  =  IWork [0] 
counter  =  IWork [1]  ; 

if  (  enable  !=  enable_upd  ) 

{ 

if  (enable  ==  0  ) 

{ 

enable_upd  =  0  ; 
counter  =  0  ; 

} 

if  (enable  ==  1  ) 

{ 

/*  Enable  control  updates  after  a  delay  of  (l_max+p)  steps  */ 
if  (  counter  >  l_max  +  p  ) 
enable_upd  =  1  ; 

/*  Otherwise  increment  the  counter  */ 
else 


}  /*  End  of  enable  logic  */ 

IWork [0]  =  enable_upd  ; 

IWork [1]  =  counter  ; 

/**********  Shift  the  eta  time  history  ***********/ 
max_del  =  l_max+p+2; 

for  (  i  =  (max_del) *Q-1  ;  i  >  Q-l  ;  i —  ) 

RWork [ WK_ETA+ i ]  =  RWork[WK_ETA+i-Q] ; 

/************  store  the  new  eta  data  *************/ 

for  (  i  =  0  ;  i  <  Q  ;  i++  ) 

RWork [WK_ETA+i]  =  *u[i]; 

/**********  Filter  eta  if  enabled  ***************/ 

if  (  filt_enable  ==  1  ) 

{ 

/*  Shift  filtered  eta  history,  then  calc,  new  filtered  value  */ 

for  (  i  =  max_del  ;  i  >  0  ;  i —  ) 

RWork  [WK_FILT_ETA+i]  =  RWork [WK_JFILT_ETA+i-l ]  ; 

RWork  [WK_FILT__ETA]  =  0.0; 
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for  (  i  =  1  ;  i  <=  filt_ord  ;  i++  ) 

RWork [WK_FILT_ETA]  -=  f ilt_alpha [ i]  *  RWork [WK_FILT_ETA+i ] 

for  (  i  =  0  ;  i  <=  filt_ord  ;  i++  ) 

RWork [WK_FILT_ETA]  +=  filt_beta[i]  * 

RWork [WK_ETA+Q*i+basis-l] ; 

/*  Form  Basis  vector  from  filtered  scalar  eta  basis  */ 

for  (  i  =  0  ;  i  <  max_del  ;  i++  ) 

eta_star[i]  =  RWork [WK_FILT_ETA+i] ; 

} 

else 

{ 

/*  Form  scalar  basis  vector  directly  from  unfiltered  eta  basis  */ 

for  (  i  =  0  ;  i  <  max_del  ;  i++  ) 

eta_star [i]  =  RWork [WK_ETA+Q* i+basis-1 ] ; 

} 


/*********  Calculate  the  phi  matrix  ***********/ 
1=1;  1  <=  N  ;  1++  ) 


for 

{ 


for  (  i  =  0  ;  i  <  Q  ;  i++  ) 

{ 

for  (  j  =  0  ;  j  <  M  ;  j++  ) 

{ 

Phi [ (1-1) *M+j ] [i]  =  0.0; 


/ *  rows  * / 

/*  columns  */ 


for  (  k  =  0  ;  k  <  p  ;  k++  ) 

{ 

Phi [ (1-1) *M+j ] [i]  +=  beta[k*Q*M  +  j*Q  +i] 

*eta_star [delay [ 1-1 ]+k+l] ; 

} 


/***********  Calculate  the  eta  residual  ************/ 

for  (  i  =  0  ;  i  <  Q  ;  i++  ) 

{ 

eta_hat[i]  =  0.0; 

for  (  j  =  0  ;  j  <  N_C0EF  ;  j++  ) 

eta_hat [ i ]  +=  Phi [ j ] [ i ]  *  RWork [WK_COEF  +  j]; 

/*  The  expected  value  of  the  disturbance  effect  */ 

res[i]  =  -(*u[i])  -  eta_hat[i]; 

/*  Calculate  the  residuals  */ 

y_res [ i ]  =  res [ i ] ; 
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} 


/*  Output  the  residuals  */ 


/************  calculate  the  update  gain  matrix  *********************/ 


for  (  i  =  0  ;  i  <  N_COEF  ;  i++  ) 

{ 

for  (  j  =  0  ;  j  <  Q  ;  j++  ) 

{ 

temp[i] [ j]  =  0.0; 


} 


for  (  k  =  0  ;  k  <  N_COEF  ;  k++  ) 

temp [ i ] [ j ]  +=  RWork [ i  +  N_COEF*k]  *  Phi[k][j]; 

/*  =  P*Phi  (N_COEF  x  Q)  */ 


/*  Prepare  "a_local"  (fi'*P*fi  +  lambda*eye (q,  q) )  for  inversion. 

*  This  routine  uses  indeces  from  l..n  (as  opposed  to  0..n-l), 

*  so  the  first  row  and  column  will  be  zeroed  out  before  proceding. 
*/ 


for  (  i  =  0  ;  i  <=  Q  ;  i++  ) 
a_local [0] [i]  =  0.0; 


for  (  i  =  1  ;  i  <=  Q  ;  i++  ) 
a_local[i] [0]  =  0.0; 


for  (  i  =  1  ;  i  <=  Q  ;  i++  ) 

{ 

for  (  j  =  1  ;  j  <=  Q  ;  j++  ) 

{ 

a_local [i] [ j ]  =  0.0; 
if  (i  ==  j) 

a_local [i] [ j ]  =  lambda; 


} 


for  (  k  =  0  ;  k  <  N_COEF  ;  k++  ) 

a_local [i] [ j ]  +=  Phi[k][i-1]  *  temp [k] [ j -1] ; 

/*  Phi'*P*Phi  +  lambda*eye (q,  q)  */ 


/*  Matrix  Inversion  code  omitted  -  it  is  exactly  the  same  as 
*  that  in  the  Sine/Cosine  Method's  code. 

*/ 


/*  Now  that  Inversion  is  done,  continue  with  Gain  update 
*  (Remember  that  *ai  pointer  is  7x7,  not  6x6  (when  m=6) 
*/ 

for  (  i  =  0  ;  i  <  N_COEF  ;  i++  ) 


for  (  j  =  0  ;  j  <  Q  ;  j++  ) 

{ 

gain[i] [j]  =  0.0; 
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for  (  k  =  0  ;  k  <  Q  ;  k++  ) 

gain[i][j]  +=  temp[i] [k]  *  ainv[k+l] [j+1] ; 

/*  L=P*fi ' *inv(fi ' *P*fi  +  lambda*eye (q,  q) )  */ 

} 

} 

}  /*  End  coeff  update  enable  (starts  prior  to  matrix  inv)  */ 

/*********  Update  the  parameter  estimates  ************/ 
for  (  i  =  0  ;  i  <  N_COEF  ;  i++  ) 

{ 

if  (  enable_upd  ==  1  ) 

{ 

for  (  j  =  0  ;  j  <  Q  ;  j++  ) 

RWork [WK_COEF  +  i]  +=  gain[i][j]  *  restj]; 

}  /*  End  coefficient  update  enable  */ 

y_coef [i]  =  RWork [WK_COEF  +  i] ; 

/*  Put  the  coefficients  in  the  output  vector  */ 

} 


/**********  update  the  covariance  matrix  ************/ 
if  (  enable_upd  ==  1  ) 

{ 

for  (  i  =  0  ;  i  <  N_COEF  ;  i++  ) 

{ 

for  (  j  =  i  ;  j  <  N_COEF  ;  j++  ) 

{ 

p_del  =  0.0; 

for  (  k  =  0  ;  k  <  Q  ;  k++  ) 

p_del  +=  gain[i][k]  *  temp[j][k]; 

/*  Assumes  cov  matrix  is  symmetric  */ 
/*  ie,  temp[j][k]  represents  Phi'*P  */ 


RWork [i  +  N_COEF* j ]  -=  p_del; 

RWork [i  +  N_C0EF* j ]  /=  lambda; 

/*  Employ  forgetting  factor  */ 


} 


if  (  i  !=  j  ) 

RWork [ j  +  N_COEF*i]  =  RWork[i  +  N_C0EF* j ] ; 

/*  Force  Symmetry  */ 


/*End  coefficient  update  enable  */ 


/*  Calculate  the  feedforward  control,  if  enabled  (else  =0)  */ 
if  (  on_off  ==  1  ) 

{ 

for  (  j  =  0  ;  j  <  M  ;  j++  ) 

{ 

y_cont[j]  =  0.0; 

for  (  i  =  0  ;  i  <  N  ;  i++  ) 
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y_cont [ j ]  -=  RWork [WK_COEF  +  (i)*M  +j]  * 

eta_star [delay [i] ] ; 

/*  Note:  Minus  sign  (-=)  above  is  to  counteract  */ 

/*  minus  sign  in  summing  block  in  Simulink  diagram  */ 

} 

} 

else 

{ 

for  (  j  =  0  ;  j  <  M  ;  j++  ) 

{ 

y_cont[j]  =  0.0; 

} 

} 

}  /**********  End  of  mdlOutputs  ***************/ 

#def ine  MDLJCJPDATE 
#if  defined (MDL_UPDATE) 

static  void  mdlUpdate (SimStruct  *S,  int_T  tid) 

{ 

} 

#endif  /*  MDL_UPDATE  */ 

#define  MDL_DERIVATIVES 
#if  defined (MDL_DERIVATIVES) 

static  void  mdlDerivatives (SimStruct  *S) 

{ 

} 

#endif  /*  MDL_DERI VAT IVES  */ 

static  void  mdlTerminate (SimStruct  *S) 

{ 

} 


*  Required  S-function  trailer  * 
*=============================*/ 


#ifdef  MATL AB_MEX_F I L E  /*  Is  this  file  being  compiled  as  a  MEX-file?  */ 
((include  "simulink. c"  /*  MEX-file  interface  mechanism  */ 

#else 

# include  “cg_sfun.h" 

#endif 


/*  Code  generation  registration  function  */ 
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